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

Merge branch 'master' of https://github.com/diydrones/ardupilot

Browse Source
This commit is contained in:
Rustom Jehangir 2016-01-08 21:07:48 -08:00
commit ee4a714a19
107 changed files with 3548 additions and 436 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
APMrover2/Log.cpp
View File

@ -256,9 +256,10 @@ struct PACKED log_Nav_Tuning {
uint16_t target_bearing_cd;
uint16_t nav_bearing_cd;
int8_t throttle;
float xtrack_error;
};
// Write a navigation tuning packet. Total length : 18 bytes
// Write a navigation tuning packet
void Rover::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
@ -268,7 +269,8 @@ void Rover::Log_Write_Nav_Tuning()
wp_distance : wp_distance,
target_bearing_cd : (uint16_t)nav_controller->target_bearing_cd(),
nav_bearing_cd : (uint16_t)nav_controller->nav_bearing_cd(),
throttle : (int8_t)(100 * channel_throttle->norm_output())
throttle : (int8_t)(100 * channel_throttle->norm_output()),
xtrack_error : nav_controller->crosstrack_error()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -393,7 +395,7 @@ const LogStructure Rover::log_structure[] = {
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qhcchf", "TimeUS,Steer,Roll,Pitch,ThrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "QHfHHb", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr" },
"NTUN", "QHfHHbf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,Thr,XT" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "QfHHHbHCb", "TimeUS,LatAcc,S1Dist,S2Dist,DCnt,TAng,TTim,Spd,Thr" },
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),

12
APMrover2/Parameters.cpp
View File

@ -582,13 +582,13 @@ void Rover::load_parameters(void)
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println("done.");
} else {
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
}
cliSerial->printf("load_all took %luus\n", micros() - before);
}
unsigned long before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
cliSerial->printf("load_all took %luus\n", micros() - before);
// set a more reasonable default NAVL1_PERIOD for rovers
L1_controller.set_default_period(8);

10
AntennaTracker/Parameters.cpp
View File

@ -302,10 +302,10 @@ void Tracker::load_parameters(void)
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
hal.console->println("done.");
} else {
uint32_t before = AP_HAL::micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before));
}
uint32_t before = AP_HAL::micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
hal.console->printf("load_all took %luus\n", (unsigned long)(AP_HAL::micros() - before));
}

1
ArduCopter/APM_Config.h
View File

@ -40,6 +40,7 @@
//#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
//#define PRECISION_LANDING ENABLED // enable precision landing using companion computer or IRLock sensor
//#define GNDEFFECT_COMPENSATION ENABLED // enable ground effect compensation for barometer (if propwash interferes with the barometer on the ground)
//#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes
// other settings
//#define THROTTLE_IN_DEADBAND 100 // redefine size of throttle deadband in pwm (0 ~ 1000)

1
ArduCopter/Copter.cpp
View File

@ -49,7 +49,6 @@ Copter::Copter(void) :
guided_mode(Guided_TakeOff),
rtl_state(RTL_InitialClimb),
rtl_state_complete(false),
rtl_alt(0.0f),
circle_pilot_yaw_override(false),
simple_cos_yaw(1.0f),
simple_sin_yaw(0.0f),

13
ArduCopter/Copter.h
View File

@ -340,7 +340,15 @@ private:
// RTL
RTLState rtl_state; // records state of rtl (initial climb, returning home, etc)
bool rtl_state_complete; // set to true if the current state is completed
float rtl_alt; // altitude the vehicle is returning at
struct {
// NEU w/ origin-relative altitude
Vector3f origin_point;
Vector3f climb_target;
Vector3f return_target;
Vector3f descent_target;
bool land;
} rtl_path;
// Circle
bool circle_pilot_yaw_override; // true if pilot is overriding yaw
@ -802,7 +810,8 @@ private:
void rtl_descent_run();
void rtl_land_start();
void rtl_land_run();
float get_RTL_alt();
void rtl_build_path();
float rtl_compute_return_alt_above_origin(float rtl_return_dist);
bool sport_init(bool ignore_checks);
void sport_run();
bool stabilize_init(bool ignore_checks);

35
ArduCopter/Parameters.cpp
View File

@ -127,6 +127,15 @@ const AP_Param::Info Copter::var_info[] = {
// @Increment: 1
// @User: Standard
GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT),
// @Param: RTL_CONE_SLOPE
// @DisplayName: RTL cone slope
// @Description: Defines a cone above home which determines maximum climb
// @Range: 0.5 10.0
// @Increment: .1
// @Values: 0:Disabled,1:Shallow,3:Steep
// @User: Standard
GSCALAR(rtl_cone_slope, "RTL_CONE_SLOPE", RTL_CONE_SLOPE),
// @Param: RTL_SPEED
// @DisplayName: RTL speed
@ -394,42 +403,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, 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
// @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, 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
// @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, 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
// @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, 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
// @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, 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
// @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, 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
// @User: Standard
GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING),
@ -1056,7 +1065,7 @@ const AP_Param::Info Copter::var_info[] = {
#endif
#if PRECISION_LANDING == ENABLED
// @Group: PRECLAND_
// @Group: PLND_
// @Path: ../libraries/AC_PrecLand/AC_PrecLand.cpp
GOBJECT(precland, "PLND_", AC_PrecLand),
#endif
@ -1145,11 +1154,11 @@ void Copter::load_parameters(void)
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println("done.");
} else {
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

4
ArduCopter/Parameters.h
View File

@ -217,7 +217,8 @@ public:
// 135 : reserved for Solo until features merged with master
//
k_param_rtl_speed_cms = 135,
k_param_fs_batt_curr_rtl, // 136
k_param_fs_batt_curr_rtl,
k_param_rtl_cone_slope, // 137
//
// 140: Sensor parameters
@ -380,6 +381,7 @@ public:
AP_Int16 rtl_altitude;
AP_Int16 rtl_speed_cms;
AP_Float rtl_cone_slope;
AP_Float sonar_gain;
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled

6
ArduCopter/ReleaseNotes.txt
View File

@ -1,5 +1,11 @@
APM:Copter Release Notes:
------------------------------------------------------------------
Copter 3.3.3-rc1 4-Jan-2016
Changes from 3.3.2
1) Restrict mode changes in helicopter when rotor is not at speed
2) add ATC_ANGLE_BOOST param to allow disabling angle boost for all flight modes
3) add LightWare range finder support
------------------------------------------------------------------
Copter 3.3.2 01-Dec-2015 / 3.3.2-rc2 18-Nov-2015
Changes from 3.3.2-rc1
1) Bug fix for desired climb rate initialisation that could lead to drop when entering AltHold, Loiter, PosHold

12
ArduCopter/config.h
View File

@ -492,6 +492,18 @@
# define RTL_CLIMB_MIN_DEFAULT 0 // vehicle will always climb this many cm as first stage of RTL
#endif
#ifndef RTL_ABS_MIN_CLIMB
# define RTL_ABS_MIN_CLIMB 250 // absolute minimum initial climb
#endif
#ifndef RTL_CONE_SLOPE
# define RTL_CONE_SLOPE 3.0f // slope of RTL cone (height / distance). 0 = No cone
#endif
#ifndef RTL_MIN_CONE_SLOPE
# define RTL_MIN_CONE_SLOPE 0.5f // minimum slope of cone
#endif
#ifndef RTL_LOITER_TIME
# define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent
#endif

100
ArduCopter/control_rtl.cpp
View File

@ -13,6 +13,7 @@
bool Copter::rtl_init(bool ignore_checks)
{
if (position_ok() || ignore_checks) {
rtl_build_path();
rtl_climb_start();
return true;
}else{
@ -34,10 +35,10 @@ void Copter::rtl_run()
rtl_loiterathome_start();
break;
case RTL_LoiterAtHome:
if (g.rtl_alt_final > 0 && !failsafe.radio) {
rtl_descent_start();
}else{
if (rtl_path.land || failsafe.radio) {
rtl_land_start();
}else{
rtl_descent_start();
}
break;
case RTL_FinalDescent:
@ -79,7 +80,6 @@ void Copter::rtl_climb_start()
{
rtl_state = RTL_InitialClimb;
rtl_state_complete = false;
rtl_alt = get_RTL_alt();
// initialise waypoint and spline controller
wp_nav.wp_and_spline_init();
@ -89,22 +89,8 @@ void Copter::rtl_climb_start()
wp_nav.set_speed_xy(g.rtl_speed_cms);
}
// get horizontal stopping point
Vector3f destination;
wp_nav.get_wp_stopping_point_xy(destination);
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
destination.z = pv_alt_above_origin(rally_point.alt);
#else
destination.z = pv_alt_above_origin(rtl_alt);
#endif
// set the destination
wp_nav.set_wp_destination(destination);
wp_nav.set_wp_destination(rtl_path.climb_target);
wp_nav.set_fast_waypoint(true);
// hold current yaw during initial climb
@ -117,19 +103,7 @@ void Copter::rtl_return_start()
rtl_state = RTL_ReturnHome;
rtl_state_complete = false;
// set target to above home/rally point
#if AC_RALLY == ENABLED
// rally_point will be the nearest rally point or home. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, rtl_alt+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
Vector3f destination = pv_location_to_vector(rally_point);
#else
Vector3f destination = pv_location_to_vector(ahrs.get_home());
destination.z = pv_alt_above_origin(rtl_alt);
#endif
wp_nav.set_wp_destination(destination);
wp_nav.set_wp_destination(rtl_path.return_target);
// initialise yaw to point home (maybe)
set_auto_yaw_mode(get_default_auto_yaw_mode(true));
@ -313,14 +287,14 @@ void Copter::rtl_descent_run()
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
// call z-axis position controller
pos_control.set_alt_target_with_slew(pv_alt_above_origin(g.rtl_alt_final), G_Dt);
pos_control.set_alt_target_with_slew(rtl_path.descent_target.z, G_Dt);
pos_control.update_z_controller();
// roll & pitch from waypoint controller, yaw rate from pilot
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
// check if we've reached within 20cm of final altitude
rtl_state_complete = fabsf(pv_alt_above_origin(g.rtl_alt_final) - inertial_nav.get_altitude()) < 20.0f;
rtl_state_complete = fabsf(rtl_path.descent_target.z - inertial_nav.get_altitude()) < 20.0f;
}
// rtl_loiterathome_start - initialise controllers to loiter over home
@ -415,13 +389,50 @@ void Copter::rtl_land_run()
rtl_state_complete = ap.land_complete;
}
// get_RTL_alt - return altitude which vehicle should return home at
// altitude is in cm above home
float Copter::get_RTL_alt()
void Copter::rtl_build_path()
{
// origin point is our stopping point
pos_control.get_stopping_point_xy(rtl_path.origin_point);
pos_control.get_stopping_point_z(rtl_path.origin_point);
// set return target to nearest rally point or home position
#if AC_RALLY == ENABLED
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0);
rtl_path.return_target = pv_location_to_vector(rally_point);
#else
rtl_path.return_target = pv_location_to_vector(ahrs.get_home());
#endif
Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point;
float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y);
// compute return altitude
rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist);
// climb target is above our origin point at the return altitude
rtl_path.climb_target.x = rtl_path.origin_point.x;
rtl_path.climb_target.y = rtl_path.origin_point.y;
rtl_path.climb_target.z = rtl_path.return_target.z;
// descent target is below return target at rtl_alt_final
rtl_path.descent_target.x = rtl_path.return_target.x;
rtl_path.descent_target.y = rtl_path.return_target.y;
rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final);
// set land flag
rtl_path.land = g.rtl_alt_final <= 0;
}
// return altitude in cm above origin at which vehicle should return home
float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist)
{
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), g.rtl_altitude);
ret = MAX(ret, RTL_ALT_MIN);
float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes
ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB)));
}
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
@ -430,6 +441,13 @@ float Copter::get_RTL_alt()
}
#endif
return ret;
}
#if AC_RALLY == ENABLED
// rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes
Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt);
rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home
rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home
ret = rally_point.alt;
#endif
return pv_alt_above_origin(ret);
}

5
ArduCopter/defines.h
View File

@ -65,7 +65,10 @@ enum aux_sw_func {
AUXSW_LOST_COPTER_SOUND = 30, // Play lost copter sound
AUXSW_MOTOR_ESTOP = 31, // Emergency Stop Switch
AUXSW_MOTOR_INTERLOCK = 32, // Motor On/Off switch
AUXSW_BRAKE = 33 // Brake flight mode
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)
};
// Frame types

13
ArduCopter/sensors.cpp
View File

@ -179,6 +179,12 @@ void Copter::compass_cal_update()
if (!hal.util->get_soft_armed()) {
compass.compass_cal_update();
}
#ifdef CAL_ALWAYS_REBOOT
if (compass.compass_cal_requires_reboot()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
#endif
}
void Copter::accel_cal_update()
@ -192,6 +198,13 @@ void Copter::accel_cal_update()
if(ins.get_new_trim(trim_roll, trim_pitch)) {
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
#ifdef CAL_ALWAYS_REBOOT
if (ins.accel_cal_requires_reboot()) {
hal.scheduler->delay(1000);
hal.scheduler->reboot(false);
}
#endif
}
#if EPM_ENABLED == ENABLED

15
ArduCopter/switches.cpp
View File

@ -235,7 +235,6 @@ void Copter::init_aux_switch_function(int8_t ch_option, uint8_t ch_flag)
case AUXSW_MISSION_RESET:
case AUXSW_ATTCON_FEEDFWD:
case AUXSW_ATTCON_ACCEL_LIM:
case AUXSW_RELAY:
case AUXSW_LANDING_GEAR:
case AUXSW_MOTOR_ESTOP:
case AUXSW_MOTOR_INTERLOCK:
@ -521,7 +520,19 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
ServoRelayEvents.do_set_relay(0, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
case AUXSW_RELAY2:
ServoRelayEvents.do_set_relay(1, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY3:
ServoRelayEvents.do_set_relay(2, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_RELAY4:
ServoRelayEvents.do_set_relay(3, ch_flag == AUX_SWITCH_HIGH);
break;
case AUXSW_LANDING_GEAR:
switch (ch_flag) {
case AUX_SWITCH_LOW:
landinggear.set_cmd_mode(LandingGear_Deploy);

60
ArduPlane/ArduPlane.cpp
View File

@ -35,11 +35,11 @@
const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(read_radio, 50, 700),
SCHED_TASK(check_short_failsafe, 50, 1000),
SCHED_TASK(ahrs_update, 50, 6400),
SCHED_TASK(ahrs_update, 400, 6400),
SCHED_TASK(update_speed_height, 50, 1600),
SCHED_TASK(update_flight_mode, 50, 1400),
SCHED_TASK(stabilize, 50, 3500),
SCHED_TASK(set_servos, 50, 1600),
SCHED_TASK(update_flight_mode, 400, 1400),
SCHED_TASK(stabilize, 400, 3500),
SCHED_TASK(set_servos, 400, 1600),
SCHED_TASK(read_control_switch, 7, 1000),
SCHED_TASK(gcs_retry_deferred, 50, 1000),
SCHED_TASK(update_GPS_50Hz, 50, 2500),
@ -172,6 +172,9 @@ void Plane::ahrs_update()
// frame yaw rate
steer_state.locked_course_err += ahrs.get_yaw_rate_earth() * G_Dt;
steer_state.locked_course_err = wrap_PI(steer_state.locked_course_err);
// update inertial_nav for quadplane
quadplane.inertial_nav.update(G_Dt);
}
/*
@ -483,7 +486,9 @@ void Plane::handle_auto_mode(void)
nav_cmd_id = auto_rtl_command.id;
}
if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
if (quadplane.in_vtol_auto()) {
quadplane.control_auto(next_WP_loc);
} else if (nav_cmd_id == MAV_CMD_NAV_TAKEOFF ||
(nav_cmd_id == MAV_CMD_NAV_LAND && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT)) {
takeoff_calc_roll();
takeoff_calc_pitch();
@ -532,6 +537,16 @@ void Plane::update_flight_mode(void)
steer_state.hold_course_cd = -1;
}
// ensure we are fly-forward
if (effective_mode == QSTABILIZE ||
effective_mode == QHOVER ||
effective_mode == QLOITER ||
quadplane.in_vtol_auto()) {
ahrs.set_fly_forward(false);
} else {
ahrs.set_fly_forward(true);
}
switch (effective_mode)
{
case AUTO:
@ -684,6 +699,23 @@ void Plane::update_flight_mode(void)
steering_control.steering = steering_control.rudder = channel_rudder->pwm_to_angle();
break;
//roll: -13788.000, pitch: -13698.000, thr: 0.000, rud: -13742.000
case QSTABILIZE:
case QHOVER:
case QLOITER: {
// 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);
float pitch_input = channel_pitch->norm_input();
if (pitch_input > 0) {
nav_pitch_cd = pitch_input * aparm.pitch_limit_max_cd;
} else {
nav_pitch_cd = -(pitch_input * pitch_limit_min_cd);
}
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
break;
}
case INITIALISING:
// handled elsewhere
@ -749,6 +781,9 @@ void Plane::update_navigation()
case AUTOTUNE:
case FLY_BY_WIRE_B:
case CIRCLE:
case QSTABILIZE:
case QHOVER:
case QLOITER:
// nothing to do
break;
}
@ -789,6 +824,7 @@ void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
case AP_SpdHgtControl::FLIGHT_NORMAL:
case AP_SpdHgtControl::FLIGHT_VTOL:
case AP_SpdHgtControl::FLIGHT_TAKEOFF:
break;
}
@ -831,7 +867,9 @@ void Plane::update_flight_stage(void)
// Update the speed & height controller states
if (auto_throttle_mode && !throttle_suppressed) {
if (control_mode==AUTO) {
if (auto_state.takeoff_complete == false) {
if (quadplane.in_vtol_auto()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else if (auto_state.takeoff_complete == false) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
@ -852,7 +890,8 @@ void Plane::update_flight_stage(void)
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
}
} else if (quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
@ -870,6 +909,13 @@ void Plane::update_flight_stage(void)
if (should_log(MASK_LOG_TECS)) {
Log_Write_TECS_Tuning();
}
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
quadplane.in_assisted_flight()) {
set_flight_stage(AP_SpdHgtControl::FLIGHT_VTOL);
} else {
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
}
// tell AHRS the airspeed to true airspeed ratio

27
ArduPlane/Attitude.cpp
View File

@ -140,6 +140,9 @@ void Plane::stabilize_stick_mixing_direct()
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == TRAINING) {
return;
}
@ -159,6 +162,9 @@ void Plane::stabilize_stick_mixing_fbw()
control_mode == AUTOTUNE ||
control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE ||
control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER ||
control_mode == TRAINING ||
(control_mode == AUTO && g.auto_fbw_steer)) {
return;
@ -354,6 +360,10 @@ void Plane::stabilize()
stabilize_training(speed_scaler);
} else if (control_mode == ACRO) {
stabilize_acro(speed_scaler);
} else if (control_mode == QSTABILIZE ||
control_mode == QHOVER ||
control_mode == QLOITER) {
quadplane.control_run();
} else {
if (g.stick_mixing == STICK_MIXING_FBW && control_mode != STABILIZE) {
stabilize_stick_mixing_fbw();
@ -628,6 +638,11 @@ bool Plane::suppress_throttle(void)
}
}
if (quadplane.is_flying()) {
gcs_send_text_fmt(MAV_SEVERITY_INFO, "Throttle enabled VTOL");
throttle_suppressed = false;
}
// throttle remains suppressed
return true;
}
@ -760,6 +775,9 @@ void Plane::set_servos(void)
{
int16_t last_throttle = channel_throttle->radio_out;
// do any transition updates for quadplane
quadplane.update();
if (control_mode == AUTO && auto_state.idle_mode) {
// special handling for balloon launch
set_servos_idle();
@ -915,6 +933,13 @@ 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()) {
// no forward throttle for now
channel_throttle->servo_out = 0;
channel_throttle->calc_pwm();
} else {
// normal throttle calculation based on servo_out
channel_throttle->calc_pwm();
@ -1083,7 +1108,7 @@ void Plane::demo_servos(uint8_t i)
void Plane::adjust_nav_pitch_throttle(void)
{
uint8_t throttle = throttle_percentage();
if (throttle < aparm.throttle_cruise) {
if (throttle < aparm.throttle_cruise && flight_stage != AP_SpdHgtControl::FLIGHT_VTOL) {
float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise;
nav_pitch_cd -= g.stab_pitch_down * 100.0f * p;
}

10
ArduPlane/GCS_Mavlink.cpp
View File

@ -39,6 +39,9 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
case FLY_BY_WIRE_A:
case AUTOTUNE:
case FLY_BY_WIRE_B:
case QSTABILIZE:
case QHOVER:
case QLOITER:
case CRUISE:
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
break;
@ -169,6 +172,9 @@ void Plane::send_extended_status1(mavlink_channel_t chan)
case STABILIZE:
case FLY_BY_WIRE_A:
case AUTOTUNE:
case QSTABILIZE:
case QHOVER:
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
break;
@ -1517,6 +1523,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
#endif
case MAV_CMD_DO_VTOL_TRANSITION:
result = plane.quadplane.handle_do_vtol_transition(packet);
break;
default:
break;
}

8
ArduPlane/Log.cpp
View File

@ -280,9 +280,10 @@ struct PACKED log_Nav_Tuning {
int16_t airspeed_cm;
float altitude;
uint32_t groundspeed_cm;
float xtrack_error;
};
// Write a navigation tuning packe
// Write a navigation tuning packet
void Plane::Log_Write_Nav_Tuning()
{
struct log_Nav_Tuning pkt = {
@ -295,7 +296,8 @@ void Plane::Log_Write_Nav_Tuning()
altitude_error_cm : (int16_t)altitude_error_cm,
airspeed_cm : (int16_t)airspeed.get_airspeed_cm(),
altitude : barometer.get_altitude(),
groundspeed_cm : (uint32_t)(gps.ground_speed()*100)
groundspeed_cm : (uint32_t)(gps.ground_speed()*100),
xtrack_error : nav_controller->crosstrack_error()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
@ -483,7 +485,7 @@ static const struct LogStructure log_structure[] = {
{ LOG_CTUN_MSG, sizeof(log_Control_Tuning),
"CTUN", "Qcccchhf", "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThrOut,RdrOut,AccY" },
{ LOG_NTUN_MSG, sizeof(log_Nav_Tuning),
"NTUN", "QCfccccfI", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM" },
"NTUN", "QCfccccfIf", "TimeUS,Yaw,WpDist,TargBrg,NavBrg,AltErr,Arspd,Alt,GSpdCM,XT" },
{ LOG_SONAR_MSG, sizeof(log_Sonar),
"SONR", "QHfffBBf", "TimeUS,DistCM,Volt,BaroAlt,GSpd,Thr,Cnt,Corr" },
{ LOG_ARM_DISARM_MSG, sizeof(log_Arm_Disarm),

17
ArduPlane/Parameters.cpp
View File

@ -434,6 +434,7 @@ const AP_Param::Info Plane::var_info[] = {
// @DisplayName: Fly By Wire B altitude change rate
// @Description: This sets the rate in m/s at which FBWB and CRUISE modes will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters.
// @Range: 1 10
// @Units: m/s
// @Increment: 0.1
// @User: Standard
GSCALAR(flybywire_climb_rate, "FBWB_CLIMB_RATE", 2.0f),
@ -1034,6 +1035,10 @@ const AP_Param::Info Plane::var_info[] = {
// @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
GOBJECT(adsb, "ADSB_", AP_ADSB),
// @Group: Q_
// @Path: quadplane.cpp
GOBJECT(quadplane, "Q_", QuadPlane),
// RC channel
//-----------
// @Group: RC1_
@ -1282,11 +1287,11 @@ void Plane::load_parameters(void)
// save the current format version
g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println("done.");
} else {
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}
uint32_t before = micros();
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
cliSerial->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

1
ArduPlane/Parameters.h
View File

@ -276,6 +276,7 @@ public:
k_param_kff_pitch_to_throttle, // unused
k_param_kff_throttle_to_pitch,
k_param_scaling_speed,
k_param_quadplane,
//
// 210: flight modes

18
ArduPlane/Plane.h
View File

@ -3,8 +3,8 @@
#ifndef _PLANE_H
#define _PLANE_H
#define THISFIRMWARE "ArduPlane V3.4.1dev"
#define FIRMWARE_VERSION 3,4,1,FIRMWARE_VERSION_TYPE_DEV
#define THISFIRMWARE "ArduPlane V3.5.0beta1"
#define FIRMWARE_VERSION 3,5,0,FIRMWARE_VERSION_TYPE_BETA
/*
Lead developer: Andrew Tridgell
@ -96,6 +96,8 @@
#include <AP_Parachute/AP_Parachute.h>
#include <AP_ADSB/AP_ADSB.h>
#include "quadplane.h"
// Configuration
#include "config.h"
@ -135,6 +137,7 @@ public:
friend class GCS_MAVLINK;
friend class Parameters;
friend class AP_Arming_Plane;
friend class QuadPlane;
Plane(void);
@ -491,6 +494,9 @@ private:
// barometric altitude at start of takeoff
float baro_takeoff_alt;
// are we in VTOL mode?
bool vtol_mode:1;
} auto_state;
struct {
@ -712,7 +718,9 @@ private:
// time that rudder arming has been running
uint32_t rudder_arm_timer;
// support for quadcopter-plane
QuadPlane quadplane{ahrs};
void demo_servos(uint8_t i);
void adjust_nav_pitch_throttle(void);
void update_load_factor(void);
@ -805,6 +813,8 @@ private:
bool verify_change_alt();
bool verify_within_distance();
bool verify_altitude_wait(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
void do_loiter_at_location();
void do_take_picture();
void log_picture();
@ -979,6 +989,8 @@ private:
void do_altitude_wait(const AP_Mission::Mission_Command& cmd);
void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
void do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
void do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
void do_wait_delay(const AP_Mission::Mission_Command& cmd);
void do_change_alt(const AP_Mission::Mission_Command& cmd);

1
ArduPlane/altitude.cpp
View File

@ -64,6 +64,7 @@ void Plane::setup_glide_slope(void)
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
/*
work out if we will gradually change altitude, or try to get to

14
ArduPlane/commands_logic.cpp
View File

@ -82,6 +82,14 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
do_altitude_wait(cmd);
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
crash_state.is_crashed = false;
return quadplane.do_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
crash_state.is_crashed = false;
return quadplane.do_vtol_land(cmd);
// Conditional commands
case MAV_CMD_CONDITION_DELAY:
@ -272,6 +280,12 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
break;
#endif
case MAV_CMD_NAV_VTOL_TAKEOFF:
return quadplane.verify_vtol_takeoff(cmd);
case MAV_CMD_NAV_VTOL_LAND:
return quadplane.verify_vtol_land(cmd);
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:

5
ArduPlane/defines.h
View File

@ -63,7 +63,10 @@ enum FlightMode {
RTL = 11,
LOITER = 12,
GUIDED = 15,
INITIALISING = 16
INITIALISING = 16,
QSTABILIZE = 17,
QHOVER = 18,
QLOITER = 19
};
// type of stick mixing enabled

14
ArduPlane/events.cpp
View File

@ -27,6 +27,13 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
}
break;
case QSTABILIZE:
case QLOITER:
failsafe.saved_mode = control_mode;
failsafe.saved_mode_set = 1;
set_mode(QHOVER);
break;
case AUTO:
case GUIDED:
case LOITER:
@ -43,6 +50,7 @@ void Plane::failsafe_short_on_event(enum failsafe_state fstype)
case CIRCLE:
case RTL:
case QHOVER:
default:
break;
}
@ -74,6 +82,11 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
}
break;
case QSTABILIZE:
case QLOITER:
set_mode(QHOVER);
break;
case AUTO:
case GUIDED:
case LOITER:
@ -85,6 +98,7 @@ void Plane::failsafe_long_on_event(enum failsafe_state fstype)
break;
case RTL:
case QHOVER:
default:
break;
}

13
ArduPlane/is_flying.cpp
View File

@ -83,6 +83,10 @@ void Plane::update_is_flying_5Hz(void)
}
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// TODO: detect ground impacts
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
if (fabsf(auto_state.sink_rate) > 0.2f) {
is_flying_bool = true;
@ -121,6 +125,10 @@ void Plane::update_is_flying_5Hz(void)
isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
}
if (quadplane.is_flying()) {
is_flying_bool = true;
}
/*
update last_flying_ms so we always know how long we have not
been flying for. This helps for crash detection and auto-disarm
@ -198,6 +206,11 @@ void Plane::crash_detection_update(void)
// TODO: handle auto missions without NAV_TAKEOFF mission cmd
break;
case AP_SpdHgtControl::FLIGHT_VTOL:
// we need a totally new method for this
crashed = false;
break;
case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
crashed = true;
// when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL

6
ArduPlane/make.inc
View File

@ -49,4 +49,8 @@ LIBRARIES += AP_RSSI
LIBRARIES += AP_RPM
LIBRARIES += AP_Parachute
LIBRARIES += AP_ADSB
LIBRARIES += AP_Motors
LIBRARIES += AC_AttitudeControl
LIBRARIES += AC_PID
LIBRARIES += AP_InertialNav
LIBRARIES += AC_WPNav

1
ArduPlane/navigation.cpp
View File

@ -64,6 +64,7 @@ void Plane::navigate()
auto_state.wp_distance = get_distance(current_loc, next_WP_loc);
auto_state.wp_proportion = location_path_proportion(current_loc,
prev_WP_loc, next_WP_loc);
SpdHgt_Controller->set_path_proportion(auto_state.wp_proportion);
// update total loiter angle
loiter_angle_update();

1133
ArduPlane/quadplane.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

182
ArduPlane/quadplane.h Normal file
View File

@ -0,0 +1,182 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include <AP_Motors/AP_Motors.h>
#include <AC_PID/AC_PID.h>
#include <AC_AttitudeControl/AC_AttitudeControl_Multi.h> // Attitude control library
#include <AP_InertialNav/AP_InertialNav.h>
#include <AC_AttitudeControl/AC_PosControl.h>
#include <AC_WPNav/AC_WPNav.h>
/*
QuadPlane specific functionality
*/
class QuadPlane
{
public:
friend class Plane;
QuadPlane(AP_AHRS_NavEKF &_ahrs);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
void control_run(void);
void control_auto(const Location &loc);
bool init_mode(void);
bool setup(void);
// update transition handling
void update(void);
// set motor arming
void set_armed(bool armed);
// is VTOL available?
bool available(void) const {
return initialised;
}
// is quadplane assisting?
bool in_assisted_flight(void) const {
return available() && assisted_flight;
}
bool handle_do_vtol_transition(const mavlink_command_long_t &packet);
bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd);
bool do_vtol_land(const AP_Mission::Mission_Command& cmd);
bool verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd);
bool verify_vtol_land(const AP_Mission::Mission_Command &cmd);
bool in_vtol_auto(void);
// vtol help for is_flying()
bool is_flying(void);
// return current throttle as a percentate
uint8_t throttle_percentage(void) const {
return last_throttle * 0.1f;
}
private:
AP_AHRS_NavEKF &ahrs;
AP_Vehicle::MultiCopter aparm;
AC_PID pid_rate_roll {0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_pitch{0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 20, 0.02};
AC_P p_stabilize_roll{4.5};
AC_P p_stabilize_pitch{4.5};
AC_P p_stabilize_yaw{4.5};
AP_InertialNav_NavEKF inertial_nav{ahrs};
AC_P p_pos_xy{1};
AC_P p_alt_hold{1};
AC_P p_vel_z{5};
AC_PID pid_accel_z{0.5, 1, 0, 800, 20, 0.02};
AC_PI_2D pi_vel_xy{1.0, 0.5, 1000, 5, 0.02};
AP_MotorsQuad *motors;
AC_AttitudeControl_Multi *attitude_control;
AC_PosControl *pos_control;
AC_WPNav *wp_nav;
// maximum vertical velocity the pilot may request
AP_Int16 pilot_velocity_z_max;
// vertical acceleration the pilot may request
AP_Int16 pilot_accel_z;
// update transition handling
void update_transition(void);
// hold hover (for transition)
void hold_hover(float target_climb_rate);
// hold stabilize (for transition)
void hold_stabilize(float throttle_in);
// get desired yaw rate in cd/s
float get_pilot_desired_yaw_rate_cds(void);
// get desired climb rate in cm/s
float get_pilot_desired_climb_rate_cms(void);
// initialise throttle_wait when entering mode
void init_throttle_wait();
// main entry points for VTOL flight modes
void init_stabilize(void);
void control_stabilize(void);
void init_hover(void);
void control_hover(void);
void init_loiter(void);
void control_loiter(void);
float assist_climb_rate_cms(void);
// calculate desired yaw rate for assistance
float desired_yaw_rate_cds(void);
bool should_relax(void);
AP_Int16 transition_time_ms;
AP_Int16 rc_speed;
// min and max PWM for throttle
AP_Int16 thr_min_pwm;
AP_Int16 thr_max_pwm;
AP_Int16 throttle_mid;
// speed below which quad assistance is given
AP_Float assist_speed;
// maximum yaw rate in degrees/second
AP_Float yaw_rate_max;
// landing speed in cm/s
AP_Int16 land_speed_cms;
// alt to switch to QLAND_FINAL
AP_Float land_final_alt;
AP_Int8 enable;
bool initialised;
// timer start for transition
uint32_t transition_start_ms;
Location last_auto_target;
// last throttle value when active
float last_throttle;
const float smoothing_gain = 6;
// true if we have reached the airspeed threshold for transition
enum {
TRANSITION_AIRSPEED_WAIT,
TRANSITION_TIMER,
TRANSITION_DONE
} transition_state;
// true when waiting for pilot throttle
bool throttle_wait;
// true when quad is assisting a fixed wing mode
bool assisted_flight;
// time when motors reached lower limit
uint32_t motors_lower_limit_start_ms;
// time we last set the loiter target
uint32_t last_loiter_ms;
enum {
QLAND_POSITION,
QLAND_DESCEND,
QLAND_FINAL,
QLAND_COMPLETE
} land_state;
};

3
ArduPlane/radio.cpp
View File

@ -46,8 +46,6 @@ void Plane::init_rc_in()
channel_pitch->set_default_dead_zone(30);
channel_rudder->set_default_dead_zone(30);
channel_throttle->set_default_dead_zone(30);
update_aux();
}
/*
@ -69,6 +67,7 @@ void Plane::init_rc_out()
channel_throttle->enable_out();
}
channel_rudder->enable_out();
update_aux();
RC_Channel_aux::enable_aux_servos();
// Initialization of servo outputs

108
ArduPlane/release-notes.txt
View File

@ -1,3 +1,111 @@
Release 3.5.0beta1, 9th January 2016
------------------------------------
The ArduPilot development team is proud to announce the release of
version 3.5.0beta1 of APM:Plane. This is a major release with a lot of
changes so please read the notes carefully!
The biggest changes in this release are:
- switch to new EKF2 kalman filter for attitude and position estimation
- added support for parachutes
- added support for QuadPlanes
- support for 3 new flight boards, the QualComm Flight, the BHAT and
the PXFmini
- support for arming on moving platforms
New Kalman Filter
The 3.4 release series was the first where APM:Plane used a Kalman
Filter by default for attitude and position estimation. It works very
well, but Paul Riseborough has been working hard recently on a new
EKF variant which fixes many issues seen with the old estimator. The
key improvements are:
- support for separate filters on each IMU for multi-IMU boards
(such as the Pixhawk), giving a high degree of redundency
- much better handling of gyro drift estimation, especially on
startup
- much faster recovery from attitude estimation errors
After extensive testing of the new EKF code we decided to make it the
default for this release. You can still use the old EKF if you want to
by setting AHRS_EKF_TYPE to 1, although it is recommended that the new
EKF be used for all aircraft.
Parachute Support
This is the first release with support for parachute landings on
plane. The configuration and use of a parachute is the same as the
existing copter parachute support. See
http://copter.ardupilot.com/wiki/parachute/
Note that parachute support is considered experimental in planes.
QuadPlane Support
This release includes support for hybrid plane/multi-rotors called
QuadPlanes. More details are available in this blog post:
http://diydrones.com/profiles/blogs/quadplane-support-in-apm-plane-3-5-0
Support for 3 new Flight Boards
The porting of ArduPilot to more flight boards continues, with support
for 3 new flight boards in this release. They are:
- the BHAT board
- the PXFmini
- the QualComm Flight
More information about the list of supported boards is available here:
http://dev.ardupilot.com/wiki/supported-autopilot-controller-boards/
Startup on a moving platform
One of the benefits of the new EKF2 estimator is that it allows for
rapid estimation of gyro offset without doing a gyro calibration on
startup. This makes it possible to startup and arm on a moving
platform by setting the INS_GYR_CAL parameter to zero (to disable gyro
calibration on boot). This should be a big help when flying off boats.
That is just a taste of all of the improvements in this release. In
total the release includes over 1500 patches. Some of the other more
significant changes include:
- RPM logging
- new waf build system
- new async accel calibrator
- SITL support for quadplanes
- improved land approach logic
- better rangefinder power control
- ADSB adapter support
- dataflash over mavlink support
- settable main loop rate
- hideable parameters
- improved crash detection logic
- added optional smooth speed weighting for landing
- improved logging for dual-GPS setups
- improvements to multiple RTK GPS drivers
- numerous HAL_Linux improvements
- improved logging of CAM messages
- added support for IMU heaters in HAL_Linux
- support for RCInput over UDP in HAL_Linux
- improved EKF startup checks for GPS accuracy
- added raw IMU logging for all platforms
- added BRD_CAN_ENABLE parameter
- support FlightGear visualisation in SITL
- configurable RGB LED brightness
Many thanks to everyone who contributed to this release! The
development team is growing at a fast pace, with 57 people
contributing changes over this release cycle.
I'd like to make special mention of Tom Pittenger and Michael du
Breuil who have been doing extensive testing of the plane development
code, and also contributing a great deal of their own
improvements. Thanks!
Release 3.4.0, 24th September 2015
----------------------------------

31
ArduPlane/system.cpp
View File

@ -196,7 +196,6 @@ void Plane::init_ardupilot()
gps.init(&DataFlash, serial_manager);
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
relay.init();
@ -233,6 +232,12 @@ void Plane::init_ardupilot()
startup_ground();
quadplane.setup();
// don't initialise rc output until after quadplane is setup as
// that can change initial values of channels
init_rc_out();
// choose the nav controller
set_nav_controller();
@ -373,6 +378,9 @@ void Plane::set_mode(enum FlightMode mode)
// new mode means new loiter
loiter.start_time_ms = 0;
// assume non-VTOL mode
auto_state.vtol_mode = false;
switch(control_mode)
{
case INITIALISING:
@ -396,7 +404,7 @@ void Plane::set_mode(enum FlightMode mode)
acro_state.locked_roll = false;
acro_state.locked_pitch = false;
break;
case CRUISE:
auto_throttle_mode = true;
cruise_state.locked_heading = false;
@ -417,6 +425,7 @@ void Plane::set_mode(enum FlightMode mode)
case AUTO:
auto_throttle_mode = true;
auto_state.vtol_mode = false;
next_WP_loc = prev_WP_loc = current_loc;
// start or resume the mission, based on MIS_AUTORESET
mission.start_or_resume();
@ -443,6 +452,17 @@ void Plane::set_mode(enum FlightMode mode)
guided_WP_loc = current_loc;
set_guided_WP();
break;
case QSTABILIZE:
case QHOVER:
case QLOITER:
if (!quadplane.init_mode()) {
control_mode = previous_mode;
} else {
auto_throttle_mode = false;
auto_state.vtol_mode = true;
}
break;
}
// start with throttle suppressed in auto_throttle modes
@ -477,6 +497,9 @@ bool Plane::mavlink_set_mode(uint8_t mode)
case AUTO:
case RTL:
case LOITER:
case QSTABILIZE:
case QHOVER:
case QLOITER:
set_mode((enum FlightMode)mode);
return true;
}
@ -730,6 +753,9 @@ void Plane::frsky_telemetry_send(void)
*/
uint8_t Plane::throttle_percentage(void)
{
if (auto_state.vtol_mode) {
return quadplane.throttle_percentage();
}
// to get the real throttle we need to use norm_output() which
// returns a number from -1 to 1.
return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100);
@ -743,6 +769,7 @@ void Plane::change_arm_state(void)
Log_Arm_Disarm();
hal.util->set_soft_armed(arming.is_armed() &&
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
quadplane.set_armed(hal.util->get_soft_armed());
}
/*

5
ArduPlane/wscript
View File

@ -28,6 +28,11 @@ def build(bld):
'AP_ServoRelayEvents',
'AP_SpdHgtControl',
'AP_TECS',
'AP_InertialNav',
'AC_WPNav',
'AC_AttitudeControl',
'AP_Motors',
'AC_PID'
],
)

2
README.md
View File

@ -91,6 +91,8 @@
>> - ***Board***: VRBrain
>> - [Mike McCauley](#)
>> - ***Board***: Flymaple
>> - [Julien BERAUD](#)
>> - ***Board***: Bebop & Bebop 2
>> - [Jonathan Challinger] (https://github.com/3drobotics/ardupilot-solo)
>> - ***Vehicle***: 3DRobotics Solo ArduPilot maintainer
>> - [Gustavo José de Sousa](https://github.com/guludo)

37
Tools/CodeStyle/xmlpretty.py
View File

@ -1,9 +1,27 @@
#!/usr/bin/python
import xml.dom.minidom as minidom
from sys import exit, argv, stderr
from sys import exit, argv, stderr, stdout
import re
import argparse
dom = minidom.parse(argv[1])
parser = argparse.ArgumentParser(description="Format XML")
parser.add_argument('infile', nargs=1)
parser.add_argument('outfile', nargs='?')
args = parser.parse_args()
f = open(args.infile[0],'r')
text = f.read()
f.close()
dom = minidom.parseString(text)
def contains_only_text(node):
childNodes = node.childNodes[:]
for child in childNodes:
if child.nodeType != child.TEXT_NODE:
return False
return True
def foreach_tree(doc, root, func, level=0):
func(doc, root, level)
@ -36,7 +54,7 @@ def strip_text_completely(doc, node, level):
node.unlink()
def auto_indent(doc, node, level):
if level > 0 and node.parentNode.nodeName not in ("description", "field", "param", "include"):
if level > 0 and not contains_only_text(node.parentNode):
node.parentNode.insertBefore(doc.createTextNode("\n%s" % (" "*4*level)), node)
if node.nextSibling is None:
node.parentNode.appendChild(doc.createTextNode("\n%s" % (" "*4*(level-1))))
@ -57,5 +75,14 @@ foreach_tree(dom, dom.documentElement, strip_text_whitespace)
foreach_tree(dom, dom.documentElement, auto_indent)
foreach_tree(dom, dom.documentElement, auto_space)
print "<?xml version='1.0'?>"
print dom.documentElement.toxml()
if args.outfile is not None:
f = open(args.outfile, 'w')
f.truncate()
else:
f = stdout
f.write("<?xml version='1.0'?>\n")
f.write(dom.documentElement.toxml())
f.write("\n")
f.close()

3
Tools/autotest/ArduPlane.parm
View File

@ -1,5 +1,4 @@
EK2_ENABLE 1
AHRS_EKF_USE 1
ALT_CTRL_ALG 2
BATT_MONITOR 4
LOG_BITMASK 65535
@ -66,7 +65,6 @@ NAVL1_PERIOD 15
ACRO_LOCKING 1
ELEVON_OUTPUT 0
VTAIL_OUTPUT 0
SKIP_GYRO_CAL 1
# we need small INS_ACC offsets so INS is recognised as being calibrated
INS_ACCOFFS_X 0.001
INS_ACCOFFS_Y 0.001
@ -80,3 +78,4 @@ INS_ACC2OFFS_Z 0.001
INS_ACC2SCAL_X 1.001
INS_ACC2SCAL_Y 1.001
INS_ACC2SCAL_Z 1.001
INS_GYR_CAL 0

75
Tools/autotest/quadplane.parm Normal file
View File

@ -0,0 +1,75 @@
AHRS_EKF_TYPE 2
ARMING_RUDDER 2
ARSPD_ENABLE 1
ARSPD_FBW_MAX 35
ARSPD_FBW_MIN 13
ARSPD_USE 1
AUTOTUNE_LEVEL 8
COMPASS_OFS2_X -0.420265
COMPASS_OFS2_Y -0.726942
COMPASS_OFS2_Z 6.665476
COMPASS_OFS3_X 0
COMPASS_OFS3_Y 0
COMPASS_OFS3_Z 0
COMPASS_OFS_X -0.453570
COMPASS_OFS_Y -0.585846
COMPASS_OFS_Z 6.815743
EK2_ENABLE 1
EK2_IMU_MASK 3
EKF_ENABLE 1
FBWB_CLIMB_RATE 10
FLTMODE1 10
FLTMODE2 11
FLTMODE3 12
FLTMODE4 5
FLTMODE5 19
FLTMODE6 5
FLTMODE_CH 5
INS_ACC2OFFS_X 0.001000
INS_ACC2OFFS_Y 0.001000
INS_ACC2OFFS_Z 0.001000
INS_ACC2SCAL_X 1.001000
INS_ACC2SCAL_Y 1.001000
INS_ACC2SCAL_Z 1.001000
INS_ACCOFFS_X 0.001000
INS_ACCOFFS_Y 0.001000
INS_ACCOFFS_Z 0.001000
INS_ACCSCAL_X 1.001000
INS_ACCSCAL_Y 1.001000
INS_ACCSCAL_Z 1.001000
INS_GYR_CAL 0
KFF_RDDRMIX 0.500000
LIM_PITCH_MAX 3000
LIM_PITCH_MIN -3000
LIM_ROLL_CD 6500
NAVL1_PERIOD 14
PTCH2SRV_D 0.309954
PTCH2SRV_I 0.425000
PTCH2SRV_IMAX 4000
PTCH2SRV_P 3.646518
PTCH2SRV_RLL 1
Q_ACCEL_Z 250
Q_ANGLE_MAX 4500
Q_ASSIST_SPEED 18
Q_ENABLE 1
Q_LAND_FINAL_ALT 6
Q_LAND_SPEED 50
RALLY_INCL_HOME 0
RALLY_LIMIT_KM 5
RALLY_TOTAL 0
RC1_DZ 30
RC1_MAX 1886
RC1_MIN 1087
RC1_REV 1
RC1_TRIM 1500
RC2_REV 1
RC3_MAX 2000
RC3_MIN 1000
RC4_REV 1
RLL2SRV_D 0.141009
RLL2SRV_I 0.425000
RLL2SRV_IMAX 4000
RLL2SRV_P 1.600000
SCHED_LOOP_RATE 300
THR_MAX 100
TRIM_ARSPD_CM 2500

39
Tools/autotest/sim_vehicle.sh
View File

@ -27,6 +27,7 @@ MODEL=""
BREAKPOINT=""
OVERRIDE_BUILD_TARGET=""
DELAY_START=0
DEFAULTS_PATH=""
usage()
{
@ -232,42 +233,55 @@ EOF
}
autotest="../Tools/autotest"
[ -d "$autotest" ] || {
# we are not running from one of the standard vehicle directories. Use
# the location of the sim_vehicle.sh script to find the path
autotest=$(dirname $(readlink -e $0))
}
# modify build target based on copter frame type
case $FRAME in
+|quad)
BUILD_TARGET="sitl"
MODEL="+"
DEFAULTS_PATH="$autotest/copter_params.parm"
;;
X)
BUILD_TARGET="sitl"
EXTRA_PARM="param set FRAME 1;"
MODEL="X"
DEFAULTS_PATH="$autotest/copter_params.parm"
;;
octa*)
BUILD_TARGET="sitl-octa"
MODEL="$FRAME"
DEFAULTS_PATH="$autotest/copter_params.parm"
;;
heli*)
BUILD_TARGET="sitl-heli"
MODEL="$FRAME"
DEFAULTS_PATH="$autotest/Helicopter.parm"
;;
heli-dual)
BUILD_TARGET="sitl-heli-dual"
BUILD_TARGET="sitl-heli-dual"
EXTRA_SIM="$EXTRA_SIM --frame=heli-dual"
MODEL="heli-dual"
;;
;;
heli-compound)
BUILD_TARGET="sitl-heli-compound"
BUILD_TARGET="sitl-heli-compound"
EXTRA_SIM="$EXTRA_SIM --frame=heli-compound"
MODEL="heli-compound"
;;
;;
IrisRos)
BUILD_TARGET="sitl"
DEFAULTS_PATH="$autotest/copter_params.parm"
;;
Gazebo)
BUILD_TARGET="sitl"
EXTRA_SIM="$EXTRA_SIM --frame=Gazebo"
MODEL="$FRAME"
DEFAULTS_PATH="$autotest/copter_params.parm"
;;
CRRCSim|last_letter*)
BUILD_TARGET="sitl"
@ -277,14 +291,17 @@ case $FRAME in
BUILD_TARGET="sitl"
MODEL="$FRAME"
check_jsbsim_version
DEFAULTS_PATH="$autotest/ArduPlane.parm"
;;
quadplane*)
BUILD_TARGET="sitl"
MODEL="$FRAME"
DEFAULTS_PATH="$autotest/quadplane.parm"
;;
*-heli)
BUILD_TARGET="sitl-heli"
MODEL="$FRAME"
DEFAULTS_PATH="$autotest/Helicopter.parm"
;;
*)
MODEL="$FRAME"
@ -299,12 +316,6 @@ if [ -n "$OVERRIDE_BUILD_TARGET" ]; then
BUILD_TARGET="$OVERRIDE_BUILD_TARGET"
fi
autotest="../Tools/autotest"
[ -d "$autotest" ] || {
# we are not running from one of the standard vehicle directories. Use
# the location of the sim_vehicle.sh script to find the path
autotest=$(dirname $(readlink -e $0))
}
VEHICLEDIR="$autotest/../../$VEHICLE"
[ -d "$VEHICLEDIR" ] || {
VEHICLEDIR=$(dirname $(readlink -e $VEHICLEDIR))
@ -400,6 +411,11 @@ if [ $USE_MAVLINK_GIMBAL == 1 ]; then
cmd="$cmd --gimbal"
fi
if [ -n "$DEFAULTS_PATH" ]; then
echo "Using defaults from $DEFAULTS_PATH"
cmd="$cmd --defaults $DEFAULTS_PATH"
fi
if [ $START_HIL == 0 ]; then
if [ $USE_VALGRIND == 1 ]; then
echo "Using valgrind"
@ -437,9 +453,6 @@ options="$options --out 10.0.2.2:14550"
fi
options="$options --out 127.0.0.1:14550 --out 127.0.0.1:14551"
extra_cmd1=""
if [ $WIPE_EEPROM == 1 ]; then
extra_cmd="param forceload $autotest/$PARMS; $EXTRA_PARM; param fetch"
fi
if [ $START_ANTENNA_TRACKER == 1 ]; then
options="$options --load-module=tracker"
extra_cmd="$extra_cmd module load map; tracker set port $TRACKER_UARTA; tracker start;"

7
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -59,6 +59,13 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] = {
// IDs 8,9,10,11 RESERVED (in use on Solo)
// @Param: ANGLE_BOOST
// @DisplayName: Angle Boost
// @Description: Angle Boost increases output throttle as the vehicle leans to reduce loss of altitude
// @Values: 0:Disabled, 1:Enabled
// @User: Advanced
AP_GROUPINFO("ANGLE_BOOST", 12, AC_AttitudeControl, _angle_boost_enabled, 1),
AP_GROUPEND
};

3
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -293,6 +293,9 @@ protected:
// Enable/Disable body frame rate feed forward
AP_Int8 _rate_bf_ff_enabled;
// Enable/Disable angle boost
AP_Int8 _angle_boost_enabled;
// Intersampling period in seconds
float _dt;

4
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -16,6 +16,10 @@ float AC_AttitudeControl_Multi::get_althold_lean_angle_max() const
// throttle value should be 0 ~ 1000
float AC_AttitudeControl_Multi::get_boosted_throttle(float throttle_in)
{
if (!_angle_boost_enabled) {
_angle_boost = 0;
return throttle_in;
}
// inverted_factor is 1 for tilt angles below 60 degrees
// reduces as a function of angle beyond 60 degrees
// becomes zero at 90 degrees

1
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -8,6 +8,7 @@
extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @Param: ENABLED
// @DisplayName: Precision Land enabled/disabled and behaviour
// @Description: Precision Land enabled/disabled and behaviour
// @Values: 0:Disabled, 1:Enabled Always Land, 2:Enabled Strict

5
libraries/AP_AHRS/AP_AHRS.h
View File

@ -412,6 +412,11 @@ public:
// time that the AHRS has been up
virtual uint32_t uptime_ms(void) const = 0;
// get the selected ekf type, for allocation decisions
int8_t get_ekf_type(void) const {
return _ekf_type;
}
protected:
AHRS_VehicleClass _vehicle_class;

3
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -109,7 +109,8 @@ void AP_AHRS_NavEKF::update_EKF1(void)
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
// slight extra delay on EKF1 to prioritise EKF2 for memory
if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U) {
ekf1_started = EKF1.InitialiseFilterDynamic();
}
}

4
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -194,6 +194,10 @@ bool AP_BattMonitor::healthy(uint8_t instance) const {
return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).healthy;
}
bool AP_BattMonitor::is_powering_off(uint8_t instance) const {
return instance < AP_BATT_MONITOR_MAX_INSTANCES && _BattMonitor_STATE(instance).is_powering_off;
}
/// has_current - returns true if battery monitor instance provides current info
bool AP_BattMonitor::has_current(uint8_t instance) const
{

4
libraries/AP_BattMonitor/AP_BattMonitor.h
View File

@ -48,6 +48,7 @@ public:
struct BattMonitor_State {
uint8_t instance; // the instance number of this monitor
bool healthy; // battery monitor is communicating correctly
bool is_powering_off; // true if the battery is about to power off
float voltage; // voltage in volts
float current_amps; // current in amperes
float current_total_mah; // total current draw since start-up
@ -70,6 +71,9 @@ public:
bool healthy(uint8_t instance) const;
bool healthy() const { return healthy(AP_BATT_PRIMARY_INSTANCE); }
bool is_powering_off(uint8_t instance) const;
bool is_powering_off() const { return is_powering_off(AP_BATT_PRIMARY_INSTANCE); }
/// has_current - returns true if battery monitor instance provides current info
bool has_current(uint8_t instance) const;
bool has_current() const { return has_current(AP_BATT_PRIMARY_INSTANCE); }

1
libraries/AP_BattMonitor/AP_BattMonitor_SMBus_PX4.cpp
View File

@ -69,6 +69,7 @@ void AP_BattMonitor_SMBus_PX4::read()
_state.last_time_micros = AP_HAL::micros();
_state.current_total_mah = batt_status.discharged_mah;
_state.healthy = true;
_state.is_powering_off = batt_status.is_powering_off;
// read capacity
if ((_batt_fd >= 0) && !_capacity_updated) {

5
libraries/AP_GPS/AP_GPS.cpp
View File

@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @Param: INJECT_TO
// @DisplayName: Destination for GPS_INJECT_DATA MAVLink packets
// @Description: The GGS can send raw serial packets to inject data to multiple GPSes.
// @Values: 0:send to first GPS, 1:send to 2nd GPS, 127:send to all
// @Values: 0:send to first GPS,1:send to 2nd GPS,127:send to all
AP_GROUPINFO("INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL),
// @Param: SBP_LOGMASK
@ -101,7 +101,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
// @DisplayName: GNSS system configuration
// @Description: Bitmask for what GNSS system to use (all unchecked or zero to leave GPS as configured)
// @Values: 0:Leave as currently configured, 1:GPS-NoSBAS, 3:GPS+SBAS, 4:Galileo-NoSBAS, 6:Galileo+SBAS, 8:Beidou, 51:GPS+IMES+QZSS+SBAS (Japan Only), 64:GLONASS, 66:GLONASS+SBAS, 67:GPS+GLONASS+SBAS
// @Bitmask: 0:GPS, 1:SBAS, 2:Galileo, 3:Beidou, 4:IMES, 5:QZSS, 6:GLOSNASS
// @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("GNSS_MODE", 10, AP_GPS, _gnss_mode, 0),
@ -428,6 +428,7 @@ AP_GPS::update(void)
// update notify with gps status. We always base this on the primary_instance
AP_Notify::flags.gps_status = state[primary_instance].status;
AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats;
}
/*

2
libraries/AP_GPS/AP_GPS_SBF.h
View File

@ -53,7 +53,7 @@ private:
"srd, Moderate, UAV\n",
"sem, PVT, 5\n",
"spm, Rover, StandAlone+SBAS+DGPS+RTK\n",
"sso, Stream2, Dsk1, Rinex+Event+RawData, msec100\n"};
"sso, Stream2, Dsk1, postprocess+event, msec100\n"};
uint32_t last_hdop = 999;
uint32_t crc_error_counter = 0;

4
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -417,4 +417,8 @@
#define HAL_OS_POSIX_IO 0
#endif
#ifndef HAL_PARAM_DEFAULTS_PATH
#define HAL_PARAM_DEFAULTS_PATH NULL
#endif
#endif // __AP_HAL_BOARDS_H__

5
libraries/AP_HAL/Util.h
View File

@ -23,6 +23,11 @@ public:
virtual const char* get_custom_log_directory() { return NULL; }
virtual const char* get_custom_terrain_directory() const { return NULL; }
// get path to custom defaults file for AP_Param
virtual const char* get_custom_defaults_file() const {
return HAL_PARAM_DEFAULTS_PATH;
}
// run a debug shall on the given stream if possible. This is used
// to support dropping into a debug shell to run firmware upgrade
// commands

36
libraries/AP_HAL_Linux/RCInput.cpp
View File

@ -42,11 +42,6 @@ uint8_t RCInput::num_channels()
return _num_channels;
}
void RCInput::set_num_channels(uint8_t num)
{
_num_channels = num;
}
uint16_t RCInput::read(uint8_t ch)
{
new_rc_input = false;
@ -317,18 +312,10 @@ reset:
memset(&dsm_state, 0, sizeof(dsm_state));
}
void RCInput::_process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1)
{
if (channel < _num_channels) {
_pwm_values[channel] = width_s1; // range: 700 ~ 2300
new_rc_input = true;
}
}
/*
process a RC input pulse of the given width
*/
void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t channel)
void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1)
{
#if 0
// useful for debugging
@ -340,23 +327,14 @@ void RCInput::_process_rc_pulse(uint16_t width_s0, uint16_t width_s1, uint16_t c
fprintf(rclog, "%u %u\n", (unsigned)width_s0, (unsigned)width_s1);
}
#endif
// treat as PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
if (channel == LINUX_RC_INPUT_CHANNEL_INVALID) {
// treat as SBUS
_process_sbus_pulse(width_s0, width_s1);
// treat as PPM-sum
_process_ppmsum_pulse(width_s0 + width_s1);
// treat as SBUS
_process_sbus_pulse(width_s0, width_s1);
// treat as DSM
_process_dsm_pulse(width_s0, width_s1);
} else {
// treat as PWM
_process_pwm_pulse(channel, width_s0, width_s1);
}
// treat as DSM
_process_dsm_pulse(width_s0, width_s1);
}
/*

7
libraries/AP_HAL_Linux/RCInput.h
View File

@ -5,7 +5,6 @@
#include "AP_HAL_Linux.h"
#define LINUX_RC_INPUT_NUM_CHANNELS 16
#define LINUX_RC_INPUT_CHANNEL_INVALID (999)
class Linux::RCInput : public AP_HAL::RCInput {
public:
@ -18,8 +17,6 @@ public:
virtual void init();
bool new_input();
uint8_t num_channels();
void set_num_channels(uint8_t num);
uint16_t read(uint8_t ch);
uint8_t read(uint16_t* periods, uint8_t len);
@ -36,8 +33,7 @@ public:
protected:
void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1,
uint16_t channel = LINUX_RC_INPUT_CHANNEL_INVALID);
void _process_rc_pulse(uint16_t width_s0, uint16_t width_s1);
void _update_periods(uint16_t *periods, uint8_t len);
private:
@ -49,7 +45,6 @@ public:
void _process_ppmsum_pulse(uint16_t width);
void _process_sbus_pulse(uint16_t width_s0, uint16_t width_s1);
void _process_dsm_pulse(uint16_t width_s0, uint16_t width_s1);
void _process_pwm_pulse(uint16_t channel, uint16_t width_s0, uint16_t width_s1);
/* override state */
uint16_t _override[LINUX_RC_INPUT_NUM_CHANNELS];

182
libraries/AP_HAL_Linux/RCInput_RPI.cpp
View File

@ -30,29 +30,13 @@
#define RCIN_RPI_BUFFER_LENGTH 8
#define RCIN_RPI_SAMPLE_FREQ 500
#define RCIN_RPI_DMA_CHANNEL 0
#define RCIN_RPI_MAX_SIZE_LINE 50
#define RCIN_RPI_MAX_COUNTER 1300
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
// Same as the circle_buffer size
// See comments in "init_ctrl_data()" to understand values "2"
#define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2)
#define RCIN_RPI_SIG_HIGH 0
#define RCIN_RPI_SIG_LOW 1
// Each gpio stands for a rcinput channel,
// the first one in RcChnGpioTbl is channel 1 in receiver
static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
RPI_GPIO_5, RPI_GPIO_6, RPI_GPIO_12,
RPI_GPIO_13, RPI_GPIO_19, RPI_GPIO_20,
RPI_GPIO_21, RPI_GPIO_26
};
#define PPM_INPUT_RPI RPI_GPIO_5
#else
#define RCIN_RPI_MAX_COUNTER 1304
#define RCIN_RPI_SIG_HIGH 1
#define RCIN_RPI_SIG_LOW 0
static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
RPI_GPIO_4
};
#endif // CONFIG_HAL_BOARD_SUBTYPE
#define PPM_INPUT_RPI RPI_GPIO_4
#endif
#define RCIN_RPI_MAX_SIZE_LINE 50
//Memory Addresses
#define RCIN_RPI_RPI1_DMA_BASE 0x20007000
@ -283,26 +267,26 @@ void RCInput_RPI::init_ctrl_data()
phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
//Init dma control blocks.
/*We are transferring 8 bytes of GPIO register. Every 7th iteration we are
sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need
7 * 8 + 8 = 64 bytes of buffer. Value 7 was selected specially to have a 64-byte "block"
TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64
"blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of
vitual pages for control blocks): for every 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO
sampling, 7 control blocks for setting frequency and 1 control block for sampling timer, so,
we need 7 + 7 + 1 = 15 control blocks. For integer value, we need 15 pages of control blocks.
Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control
blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO
and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer.
/*We are transferring 1 byte of GPIO register. Every 56th iteration we are
sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need
56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block"
TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64
"blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of
vitual pages for control blocks): for every 56 iterations (64 bytes of buffer) we need 56 control blocks for GPIO
sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so,
we need 56 + 56 + 1 = 113 control blocks. For integer value, we need 113 pages of control blocks.
Each control block length is 32 bytes. In 113 pages we will have (113 * 4096 / 32) = 113 * 128 control
blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO
and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer.
*/
// fprintf(stderr, "ERROR SEARCH1\n");
uint32_t i = 0;
for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168
for (i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344
{
//Transfer timer every 7th sample
if(i % 7 == 0) {
//Transfer timer every 56th sample
if(i % 56 == 0) {
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE,
@ -316,16 +300,16 @@ void RCInput_RPI::init_ctrl_data()
cbp += sizeof(dma_cb_t);
}
// Transfer GPIO (8 byte)
// Transfer GPIO (1 byte)
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR,
(uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
8,
0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR,
(uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
1,
0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t) ) );
dest += 8;
dest += 1;
cbp += sizeof(dma_cb_t);
// Delay (for setting sampling frequency)
@ -405,9 +389,15 @@ void RCInput_RPI::set_sigaction()
//Initial setup of variables
RCInput_RPI::RCInput_RPI():
prev_tick(0),
delta_time(0),
curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ),
curr_pointer(0),
curr_channel(0)
curr_channel(0),
width_s0(0),
curr_signal(0),
last_signal(228),
state(RCIN_RPI_INITIAL_STATE)
{
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
int version = 2;
@ -416,9 +406,9 @@ RCInput_RPI::RCInput_RPI():
#endif
set_physical_addresses(version);
//Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "15"
//Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "113"
circle_buffer = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, version);
con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, version);
con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version);
}
RCInput_RPI::~RCInput_RPI()
@ -442,15 +432,12 @@ void RCInput_RPI::init_registers()
void RCInput_RPI::init()
{
uint64_t signal_states(0);
init_registers();
//Enable PPM or PWM input
for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]);
rc_channels[i].enable_pin->mode(HAL_GPIO_INPUT);
}
//Enable PPM input
enable_pin = hal.gpio->channel(PPM_INPUT_RPI);
enable_pin->mode(HAL_GPIO_INPUT);
//Configuration
set_sigaction();
@ -463,17 +450,11 @@ void RCInput_RPI::init()
//Reading first sample
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
prev_tick = curr_tick;
curr_pointer += 8;
signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
rc_channels[i].prev_tick = curr_tick;
rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH
: RCIN_RPI_SIG_LOW;
rc_channels[i].last_signal = rc_channels[i].curr_signal;
}
curr_pointer += 8;
set_num_channels(RCIN_RPI_CHN_NUM);
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
last_signal = curr_signal;
curr_pointer++;
}
@ -482,7 +463,6 @@ void RCInput_RPI::_timer_tick()
{
int j;
void* x;
uint64_t signal_states(0);
//Now we are getting address in which DMAC is writing at current moment
dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]);
@ -500,7 +480,7 @@ void RCInput_RPI::_timer_tick()
}
//Processing ready bytes
for (;counter > 0x40;) {
for (;counter > 0x40;counter--) {
//Is it timer samle?
if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
@ -508,50 +488,36 @@ void RCInput_RPI::_timer_tick()
counter-=8;
}
//Reading required bit
signal_states = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) {
rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH
: RCIN_RPI_SIG_LOW;
//If the signal changed
if (rc_channels[i].curr_signal != rc_channels[i].last_signal) {
rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick;
rc_channels[i].prev_tick = curr_tick;
switch (rc_channels[i].state) {
case RCIN_RPI_INITIAL_STATE:
rc_channels[i].state = RCIN_RPI_ZERO_STATE;
break;
case RCIN_RPI_ZERO_STATE:
if (rc_channels[i].curr_signal == 0) {
rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time;
rc_channels[i].state = RCIN_RPI_ONE_STATE;
break;
}
else {
break;
}
case RCIN_RPI_ONE_STATE:
if (rc_channels[i].curr_signal == 1) {
rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time;
rc_channels[i].state = RCIN_RPI_ZERO_STATE;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
_process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1, i);
#else
_process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1);
#endif // CONFIG_HAL_BOARD_SUBTYPE
break;
}
else{
break;
}
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
//If the signal changed
if (curr_signal != last_signal) {
delta_time = curr_tick - prev_tick;
prev_tick = curr_tick;
switch (state) {
case RCIN_RPI_INITIAL_STATE:
state = RCIN_RPI_ZERO_STATE;
break;
case RCIN_RPI_ZERO_STATE:
if (curr_signal == 0) {
width_s0 = (uint16_t) delta_time;
state = RCIN_RPI_ONE_STATE;
break;
}
else
break;
case RCIN_RPI_ONE_STATE:
if (curr_signal == 1) {
width_s1 = (uint16_t) delta_time;
state = RCIN_RPI_ZERO_STATE;
_process_rc_pulse(width_s0, width_s1);
break;
}
else
break;
}
rc_channels[i].last_signal = rc_channels[i].curr_signal;
}
curr_pointer += 8;
counter -= 8;
last_signal = curr_signal;
curr_pointer++;
if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
curr_pointer = 0;
}

35
libraries/AP_HAL_Linux/RCInput_RPI.h
View File

@ -22,11 +22,6 @@
#include <assert.h>
#include <queue>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define RCIN_RPI_CHN_NUM 8
#else
#define RCIN_RPI_CHN_NUM 1
#endif
enum state_t{
RCIN_RPI_INITIAL_STATE = -1,
@ -103,33 +98,23 @@ private:
Memory_table *con_blocks;
uint64_t curr_tick;
uint64_t prev_tick;
uint64_t delta_time;
uint32_t curr_tick_inc;
uint32_t curr_pointer;
uint32_t curr_channel;
uint32_t counter;
struct RcChannel {
RcChannel() :
prev_tick(0), delta_time(0),
width_s0(0), width_s1(0),
curr_signal(0), last_signal(0),
enable_pin(0), state(RCIN_RPI_INITIAL_STATE)
{}
uint16_t width_s0;
uint16_t width_s1;
uint64_t prev_tick;
uint64_t delta_time;
uint16_t width_s0;
uint16_t width_s1;
uint8_t curr_signal;
uint8_t last_signal;
state_t state;
AP_HAL::DigitalSource *enable_pin;
} rc_channels[RCIN_RPI_CHN_NUM];
uint8_t curr_signal;
uint8_t last_signal;
state_t state;
AP_HAL::DigitalSource *enable_pin;
void init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb);
void* map_peripheral(uint32_t base, uint32_t len);

10
libraries/AP_HAL_PX4/Storage.cpp
View File

@ -23,6 +23,7 @@ using namespace PX4;
#define STORAGE_DIR "/fs/microsd/APM"
#define OLD_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".stg"
#define OLD_STORAGE_FILE_BAK STORAGE_DIR "/" SKETCHNAME ".bak"
//#define SAVE_STORAGE_FILE STORAGE_DIR "/" SKETCHNAME ".sav"
#define MTD_PARAMS_FILE "/fs/mtd"
#define MTD_SIGNATURE 0x14012014
#define MTD_SIGNATURE_OFFSET (8192-4)
@ -164,6 +165,15 @@ void PX4Storage::_storage_open(void)
}
}
close(fd);
#ifdef SAVE_STORAGE_FILE
fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (fd != -1) {
write(fd, _buffer, sizeof(_buffer));
close(fd);
::printf("Saved storage file %s\n", SAVE_STORAGE_FILE);
}
#endif
_initialised = true;
}

2
libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
View File

@ -43,7 +43,7 @@ static SITLUARTDriver sitlUart2Driver(2, &sitlState);
static SITLUARTDriver sitlUart3Driver(3, &sitlState);
static SITLUARTDriver sitlUart4Driver(4, &sitlState);
static SITLUtil utilInstance;
static SITLUtil utilInstance(&sitlState);
HAL_SITL::HAL_SITL() :
AP_HAL::HAL(

3
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -31,6 +31,7 @@ class HAL_SITL;
class HALSITL::SITL_State {
friend class HALSITL::SITLScheduler;
friend class HALSITL::SITLUtil;
public:
void init(int argc, char * const argv[]);
@ -213,6 +214,8 @@ private:
// TCP address to connect uartC to
const char *_client_address;
const char *defaults_path = HAL_PARAM_DEFAULTS_PATH;
};
#endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL

8
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -24,6 +24,7 @@
#include <SITL/SIM_JSBSim.h>
#include <SITL/SIM_Tracker.h>
#include <SITL/SIM_Balloon.h>
#include <SITL/SIM_FlightAxis.h>
extern const AP_HAL::HAL& hal;
@ -55,6 +56,7 @@ void SITL_State::_usage(void)
"\t--uartC device set device string for UARTC\n"
"\t--uartD device set device string for UARTD\n"
"\t--uartE device set device string for UARTE\n"
"\t--defaults path set path to defaults file\n"
);
}
@ -75,6 +77,7 @@ static const struct {
{ "rover", SimRover::create },
{ "crrcsim", CRRCSim::create },
{ "jsbsim", JSBSim::create },
{ "flightaxis", FlightAxis::create },
{ "gazebo", Gazebo::create },
{ "last_letter", last_letter::create },
{ "tracker", Tracker::create },
@ -120,6 +123,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
CMDLINE_UARTD,
CMDLINE_UARTE,
CMDLINE_ADSB,
CMDLINE_DEFAULTS
};
const struct GetOptLong::option options[] = {
@ -142,6 +146,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"gimbal", false, 0, CMDLINE_GIMBAL},
{"adsb", false, 0, CMDLINE_ADSB},
{"autotest-dir", true, 0, CMDLINE_AUTOTESTDIR},
{"defaults", true, 0, CMDLINE_DEFAULTS},
{0, false, 0, 0}
};
@ -197,6 +202,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case CMDLINE_AUTOTESTDIR:
autotest_dir = strdup(gopt.optarg);
break;
case CMDLINE_DEFAULTS:
defaults_path = strdup(gopt.optarg);
break;
case CMDLINE_UARTA:
case CMDLINE_UARTB:

2
libraries/AP_HAL_SITL/UARTDriver.h
View File

@ -56,6 +56,8 @@ public:
// file descriptor, exposed so SITL_State::loop_hook() can use it
int _fd;
enum flow_control get_flow_control(void) { return FLOW_CONTROL_ENABLE; }
private:
uint8_t _portNumber;
bool _connected = false; // true if a client has connected

10
libraries/AP_HAL_SITL/Util.h
View File

@ -8,6 +8,9 @@
class HALSITL::SITLUtil : public AP_HAL::Util {
public:
SITLUtil(SITL_State *_sitlState) :
sitlState(_sitlState) {}
bool run_debug_shell(AP_HAL::BetterStream *stream) {
return false;
}
@ -22,6 +25,13 @@ public:
// create a new semaphore
AP_HAL::Semaphore *new_semaphore(void) override { return new HALSITL::Semaphore; }
// get path to custom defaults file for AP_Param
const char* get_custom_defaults_file() const override {
return sitlState->defaults_path;
}
private:
SITL_State *sitlState;
};
#endif // __AP_HAL_SITL_UTIL_H__

16
libraries/AP_Mission/AP_Mission.cpp
View File

@ -709,6 +709,14 @@ MAV_MISSION_RESULT AP_Mission::mavlink_to_mission_cmd(const mavlink_mission_item
cmd.content.altitude_wait.wiggle_time = packet.param3;
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
copy_location = true;
break;
case MAV_CMD_NAV_VTOL_LAND:
copy_location = true;
break;
default:
// unrecognised command
return MAV_MISSION_UNSUPPORTED;
@ -1041,6 +1049,14 @@ bool AP_Mission::mission_cmd_to_mavlink(const AP_Mission::Mission_Command& cmd,
packet.param3 = cmd.content.altitude_wait.wiggle_time;
break;
case MAV_CMD_NAV_VTOL_TAKEOFF:
copy_location = true;
break;
case MAV_CMD_NAV_VTOL_LAND:
copy_location = true;
break;
default:
// unrecognised command
return false;

12
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -88,23 +88,23 @@ void AP_MotorsCoax::set_update_rate( uint16_t speed_hz )
uint32_t mask2 =
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4 ;
hal.rcout->set_freq(mask2, _speed_hz);
rc_set_freq(mask2, _speed_hz);
// set update rate for the two servos
uint32_t mask =
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 ;
hal.rcout->set_freq(mask, _servo_speed);
rc_set_freq(mask, _servo_speed);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsCoax::enable()
{
// enable output channels
hal.rcout->enable_ch(AP_MOTORS_MOT_1);
hal.rcout->enable_ch(AP_MOTORS_MOT_2);
hal.rcout->enable_ch(AP_MOTORS_MOT_3);
hal.rcout->enable_ch(AP_MOTORS_MOT_4);
rc_enable_ch(AP_MOTORS_MOT_1);
rc_enable_ch(AP_MOTORS_MOT_2);
rc_enable_ch(AP_MOTORS_MOT_3);
rc_enable_ch(AP_MOTORS_MOT_4);
}
// output_min - sends minimum values out to the motor and trim values to the servos

6
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -27,10 +27,10 @@ class AP_MotorsHeli_RSC {
public:
AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn,
uint8_t default_channel,
uint16_t loop_rate) :
uint16_t loop_rate) :
_loop_rate(loop_rate),
_aux_fn(aux_fn),
_default_channel(default_channel),
_loop_rate(loop_rate)
_default_channel(default_channel)
{};
// init_servo - servo initialization on start-up

16
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -131,19 +131,19 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz )
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4;
hal.rcout->set_freq(mask, _speed_hz);
rc_set_freq(mask, _speed_hz);
}
// enable - starts allowing signals to be sent to motors and servos
void AP_MotorsHeli_Single::enable()
{
// enable output channels
hal.rcout->enable_ch(AP_MOTORS_MOT_1); // swash servo 1
hal.rcout->enable_ch(AP_MOTORS_MOT_2); // swash servo 2
hal.rcout->enable_ch(AP_MOTORS_MOT_3); // swash servo 3
hal.rcout->enable_ch(AP_MOTORS_MOT_4); // yaw
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor
hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc
rc_enable_ch(AP_MOTORS_MOT_1); // swash servo 1
rc_enable_ch(AP_MOTORS_MOT_2); // swash servo 2
rc_enable_ch(AP_MOTORS_MOT_3); // swash servo 3
rc_enable_ch(AP_MOTORS_MOT_4); // yaw
rc_enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor
rc_enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc
}
// init_outputs - initialise Servo/PWM ranges and endpoints
@ -316,7 +316,7 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors()
uint16_t AP_MotorsHeli_Single::get_motor_mask()
{
// heli uses channels 1,2,3,4,7 and 8
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC);
}
// update_motor_controls - sends commands to motor controllers

17
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -49,7 +49,7 @@ void AP_MotorsMatrix::set_update_rate( uint16_t speed_hz )
mask |= 1U << i;
}
}
hal.rcout->set_freq( mask, _speed_hz );
rc_set_freq( mask, _speed_hz );
}
// set frame orientation (normally + or X)
@ -78,7 +78,7 @@ void AP_MotorsMatrix::enable()
// enable output channels
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
if( motor_enabled[i] ) {
hal.rcout->enable_ch(i);
rc_enable_ch(i);
}
}
}
@ -114,7 +114,7 @@ uint16_t AP_MotorsMatrix::get_motor_mask()
mask |= 1U << i;
}
}
return mask;
return rc_map_mask(mask);
}
void AP_MotorsMatrix::output_armed_not_stabilizing()
@ -415,8 +415,15 @@ void AP_MotorsMatrix::add_motor_raw(int8_t motor_num, float roll_fac, float pitc
// set order that motor appears in test
_test_order[motor_num] = testing_order;
// disable this channel from being used by RC_Channel_aux
RC_Channel_aux::disable_aux_channel(motor_num);
uint8_t chan;
if (RC_Channel_aux::find_channel((RC_Channel_aux::Aux_servo_function_t)(RC_Channel_aux::k_motor1+motor_num),
chan)) {
_motor_map[motor_num] = chan;
_motor_map_mask |= 1U<<motor_num;
} else {
// disable this channel from being used by RC_Channel_aux
RC_Channel_aux::disable_aux_channel(motor_num);
}
}
}

6
libraries/AP_Motors/AP_MotorsQuad.cpp
View File

@ -88,6 +88,12 @@ void AP_MotorsQuad::setup_motors()
add_motor(AP_MOTORS_MOT_2, 0, -160, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_3, -60, -60, 0, 4);
add_motor(AP_MOTORS_MOT_4, 0, 160, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
} else if ( _flags.frame_orientation == AP_MOTORS_QUADPLANE ) {
// quadplane frame set-up, X arrangement on motors 5 to 8
add_motor(AP_MOTORS_MOT_5, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);
add_motor(AP_MOTORS_MOT_6, -135, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 3);
add_motor(AP_MOTORS_MOT_7, -45, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 4);
add_motor(AP_MOTORS_MOT_8, 135, AP_MOTORS_MATRIX_YAW_FACTOR_CW, 2);
}else{
// X frame set-up
add_motor(AP_MOTORS_MOT_1, 45, AP_MOTORS_MATRIX_YAW_FACTOR_CCW, 1);

16
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -96,20 +96,20 @@ void AP_MotorsSingle::set_update_rate( uint16_t speed_hz )
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_3 |
1U << AP_MOTORS_MOT_4 ;
hal.rcout->set_freq(mask, _servo_speed);
rc_set_freq(mask, _servo_speed);
uint32_t mask2 = 1U << AP_MOTORS_MOT_7;
hal.rcout->set_freq(mask2, _speed_hz);
rc_set_freq(mask2, _speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsSingle::enable()
{
// enable output channels
hal.rcout->enable_ch(AP_MOTORS_MOT_1);
hal.rcout->enable_ch(AP_MOTORS_MOT_2);
hal.rcout->enable_ch(AP_MOTORS_MOT_3);
hal.rcout->enable_ch(AP_MOTORS_MOT_4);
hal.rcout->enable_ch(AP_MOTORS_MOT_7);
rc_enable_ch(AP_MOTORS_MOT_1);
rc_enable_ch(AP_MOTORS_MOT_2);
rc_enable_ch(AP_MOTORS_MOT_3);
rc_enable_ch(AP_MOTORS_MOT_4);
rc_enable_ch(AP_MOTORS_MOT_7);
}
// output_min - sends minimum values out to the motor and trim values to the servos
@ -130,7 +130,7 @@ void AP_MotorsSingle::output_min()
uint16_t AP_MotorsSingle::get_motor_mask()
{
// single copter uses channels 1,2,3,4 and 7
return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
return rc_map_mask(1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << 6);
}
void AP_MotorsSingle::output_armed_not_stabilizing()

12
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -97,17 +97,17 @@ void AP_MotorsTri::set_update_rate( uint16_t speed_hz )
1U << AP_MOTORS_MOT_1 |
1U << AP_MOTORS_MOT_2 |
1U << AP_MOTORS_MOT_4;
hal.rcout->set_freq(mask, _speed_hz);
rc_set_freq(mask, _speed_hz);
}
// enable - starts allowing signals to be sent to motors
void AP_MotorsTri::enable()
{
// enable output channels
hal.rcout->enable_ch(AP_MOTORS_MOT_1);
hal.rcout->enable_ch(AP_MOTORS_MOT_2);
hal.rcout->enable_ch(AP_MOTORS_MOT_4);
hal.rcout->enable_ch(AP_MOTORS_CH_TRI_YAW);
rc_enable_ch(AP_MOTORS_MOT_1);
rc_enable_ch(AP_MOTORS_MOT_2);
rc_enable_ch(AP_MOTORS_MOT_4);
rc_enable_ch(AP_MOTORS_CH_TRI_YAW);
}
// output_min - sends minimum values out to the motors
@ -130,7 +130,7 @@ void AP_MotorsTri::output_min()
uint16_t AP_MotorsTri::get_motor_mask()
{
// tri copter uses channels 1,2,4 and 7
return (1U << AP_MOTORS_MOT_1) |
return rc_map_mask(1U << AP_MOTORS_MOT_1) |
(1U << AP_MOTORS_MOT_2) |
(1U << AP_MOTORS_MOT_4) |
(1U << AP_MOTORS_CH_TRI_YAW);

41
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -70,5 +70,46 @@ void AP_Motors::armed(bool arm)
*/
void AP_Motors::rc_write(uint8_t chan, uint16_t pwm)
{
if (_motor_map_mask & (1U<<chan)) {
// we have a mapped motor number for this channel
chan = _motor_map[chan];
}
hal.rcout->write(chan, pwm);
}
/*
set frequency of a set of channels
*/
void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz)
{
hal.rcout->set_freq(rc_map_mask(mask), freq_hz);
}
void AP_Motors::rc_enable_ch(uint8_t chan)
{
if (_motor_map_mask & (1U<<chan)) {
// we have a mapped motor number for this channel
chan = _motor_map[chan];
}
hal.rcout->enable_ch(chan);
}
/*
map an internal motor mask to real motor mask
*/
uint32_t AP_Motors::rc_map_mask(uint32_t mask) const
{
uint32_t mask2 = 0;
for (uint8_t i=0; i<32; i++) {
uint32_t bit = 1UL<<i;
if (mask & bit) {
if (_motor_map_mask & bit) {
// we have a mapped motor number for this channel
mask2 |= (1UL << _motor_map[i]);
} else {
mask2 |= bit;
}
}
}
return mask2;
}

8
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -32,6 +32,7 @@
#define AP_MOTORS_NEW_X_FRAME 11
#define AP_MOTORS_NEW_V_FRAME 12
#define AP_MOTORS_NEW_H_FRAME 13 // same as X frame but motors spin in opposite direction
#define AP_MOTORS_QUADPLANE 14 // motors on 5..8
// motor update rate
#define AP_MOTORS_SPEED_DEFAULT 490 // default output rate to the motors
@ -135,6 +136,9 @@ protected:
virtual void output_armed_zero_throttle() { output_min(); }
virtual void output_disarmed()=0;
virtual void rc_write(uint8_t chan, uint16_t pwm);
virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz);
virtual void rc_enable_ch(uint8_t chan);
virtual uint32_t rc_map_mask(uint32_t mask) const;
// update the throttle input filter
virtual void update_throttle_filter() = 0;
@ -169,5 +173,9 @@ protected:
float _batt_voltage; // latest battery voltage reading
float _batt_current; // latest battery current reading
float _air_density_ratio; // air density / sea level density - decreases in altitude
// mapping to output channels
uint8_t _motor_map[AP_MOTORS_MAX_NUM_MOTORS];
uint16_t _motor_map_mask;
};
#endif // __AP_MOTORS_CLASS_H__

51
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -32,15 +32,22 @@
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// declare functions
void setup();
void loop();
void motor_order_test();
void stability_test();
void update_motors();
RC_Channel rc1(0), rc2(1), rc3(2), rc4(3);
// uncomment the row below depending upon what frame you are using
//AP_MotorsTri motors(rc1, rc2, rc3, rc4, 400);
AP_MotorsQuad motors(rc1, rc2, rc3, rc4, 400);
//AP_MotorsHexa motors(rc1, rc2, rc3, rc4, 400);
//AP_MotorsY6 motors(rc1, rc2, rc3, rc4, 400);
//AP_MotorsOcta motors(rc1, rc2, rc3, rc4, 400);
//AP_MotorsOctaQuad motors(rc1, rc2, rc3, rc4, 400);
//AP_MotorsTri motors(400);
AP_MotorsQuad motors(400);
//AP_MotorsHexa motors(400);
//AP_MotorsY6 motors(400);
//AP_MotorsOcta motors(400);
//AP_MotorsOctaQuad motors(400);
//AP_MotorsHeli motors(rc1, rc2, rc3, rc4, 400);
@ -51,11 +58,12 @@ void setup()
// motor initialisation
motors.set_update_rate(490);
// motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.set_frame_orientation(AP_MOTORS_PLUS_FRAME);
motors.set_min_throttle(130);
motors.set_frame_orientation(AP_MOTORS_X_FRAME);
motors.Init();
motors.set_throttle_range(130,1000,2000);
motors.set_hover_throttle(500);
motors.Init(); // initialise motors
motors.enable();
motors.output_min();
// setup radio
if (rc3.radio_min == 0) {
@ -72,9 +80,6 @@ void setup()
rc3.set_range(130, 1000);
rc4.set_angle(4500);
motors.enable();
motors.output_min();
hal.scheduler->delay(1000);
}
@ -123,7 +128,7 @@ void motor_order_test()
// stability_test
void stability_test()
{
int16_t value, roll_in, pitch_in, yaw_in, throttle_in, throttle_radio_in, avg_out;
int16_t roll_in, pitch_in, yaw_in, throttle_in, avg_out;
int16_t testing_array[][4] = {
// roll, pitch, yaw, throttle
@ -166,9 +171,11 @@ void stability_test()
// arm motors
motors.armed(true);
motors.set_interlock(true);
motors.set_stabilizing(true);
// run stability test
for (int16_t i=0; i < testing_array_rows; i++) {
for (uint16_t i=0; i<testing_array_rows; i++) {
roll_in = testing_array[i][0];
pitch_in = testing_array[i][1];
yaw_in = testing_array[i][2];
@ -177,13 +184,12 @@ void stability_test()
motors.set_roll(pitch_in);
motors.set_yaw(yaw_in);
motors.set_throttle(throttle_in);
motors.output();
update_motors();
// calc average output
throttle_radio_in = rc3.radio_out;
avg_out = ((hal.rcout->read(0) + hal.rcout->read(1) + hal.rcout->read(2) + hal.rcout->read(3))/4);
// display input and output
hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t ThrIn/AvgOut:%5d/%5d\n",
hal.console->printf("R:%5d \tP:%5d \tY:%5d \tT:%5d\tMOT1:%5d \tMOT2:%5d \tMOT3:%5d \tMOT4:%5d \t AvgOut:%5d\n",
(int)roll_in,
(int)pitch_in,
(int)yaw_in,
@ -192,7 +198,6 @@ void stability_test()
(int)hal.rcout->read(1),
(int)hal.rcout->read(2),
(int)hal.rcout->read(3),
(int)throttle_radio_in,
(int)avg_out);
}
// set all inputs to motor library to zero and disarm motors
@ -205,4 +210,12 @@ void stability_test()
hal.console->println("finished test.");
}
void update_motors()
{
// call update motors 1000 times to get any ramp limiting completed
for (uint16_t i=0; i<1000; i++) {
motors.output();
}
}
AP_HAL_MAIN();

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -421,6 +421,11 @@ bool NavEKF::InitialiseFilterDynamic(void)
return false;
}
if (core == nullptr) {
if (hal.util->available_memory() < 4096 + sizeof(*core)) {
_enable.set(0);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "NavEKF: not enough memory");
return false;
}
core = new NavEKF_core(*this, _ahrs, _baro, _rng);
if (core == nullptr) {
_enable.set(0);

16
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -248,7 +248,7 @@ void NavEKF2_core::FuseVelPosNED()
// if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
// otherwise we scale it using manoeuvre acceleration
// Use different errors if flying without GPS using synthetic position and velocity data
if (PV_AidingMode == AID_NONE && inFlight) {
if (PV_AidingMode == AID_NONE && motorsArmed) {
// Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
// This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
R_OBS[0] = sq(10.0f);
@ -257,6 +257,8 @@ void NavEKF2_core::FuseVelPosNED()
// Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
R_OBS[3] = sq(25.0f);
R_OBS[4] = R_OBS[3];
R_OBS[5] = posDownObsNoise;
for (uint8_t i=0; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
} else {
if (gpsSpdAccuracy > 0.0f) {
// use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
@ -270,14 +272,14 @@ void NavEKF2_core::FuseVelPosNED()
R_OBS[1] = R_OBS[0];
R_OBS[3] = sq(constrain_float(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
R_OBS[4] = R_OBS[3];
R_OBS[5] = posDownObsNoise;
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
}
R_OBS[5] = posDownObsNoise;
// For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
// For horizontal GPs velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPs perfomrance
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
for (uint8_t i=0; i<=1; i++) R_OBS_DATA_CHECKS[i] = sq(constrain_float(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
for (uint8_t i=2; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
// if vertical GPS velocity data and an independant height source is being used, check to see if the GPS vertical velocity and altimeter
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting

4
libraries/AP_Notify/AP_Notify.cpp
View File

@ -66,8 +66,8 @@ struct AP_Notify::notify_events_type AP_Notify::events;
ToshibaLED_I2C toshibaled;
NotifyDevice *AP_Notify::_devices[] = {&boardled, &navioled, &toshibaled};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
ToshibaLED_I2C toshibaled;
NotifyDevice *AP_Notify::_devices[] = {&toshibaled};
Display_SSD1306_I2C display;
NotifyDevice *AP_Notify::_devices[] = {&display};
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
ToshibaLED_I2C toshibaled;
ToneAlarm_Linux tonealarm;

3
libraries/AP_Notify/AP_Notify.h
View File

@ -33,6 +33,8 @@
#include "VRBoard_LED.h"
#include "OreoLED_PX4.h"
#include "RCOutputRGBLed.h"
#include "Display.h"
#include "Display_SSD1306_I2C.h"
#ifndef OREOLED_ENABLED
# define OREOLED_ENABLED 0 // set to 1 to enable OreoLEDs
@ -55,6 +57,7 @@ public:
struct notify_flags_type {
uint32_t initialising : 1; // 1 if initialising and copter should not be moved
uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock
uint32_t gps_num_sats : 6; // number of sats
uint32_t armed : 1; // 0 = disarmed, 1 = armed
uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed
uint32_t pre_arm_gps_check : 1; // 0 = failing pre-arm GPS checks, 1 = passed

450
libraries/AP_Notify/Display.cpp Normal file
View File

@ -0,0 +1,450 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/* Notify display driver for 128 x 64 pixel displays */
#include "AP_Notify.h"
#include <AP_GPS/AP_GPS.h>
#include "Display.h"
static const uint8_t _font[] = {
0x00, 0x00, 0x00, 0x00, 0x00,
0x3E, 0x5B, 0x4F, 0x5B, 0x3E,
0x3E, 0x6B, 0x4F, 0x6B, 0x3E,
0x1C, 0x3E, 0x7C, 0x3E, 0x1C,
0x18, 0x3C, 0x7E, 0x3C, 0x18,
0x1C, 0x57, 0x7D, 0x57, 0x1C,
0x1C, 0x5E, 0x7F, 0x5E, 0x1C,
0x00, 0x18, 0x3C, 0x18, 0x00,
0xFF, 0xE7, 0xC3, 0xE7, 0xFF,
0x00, 0x18, 0x24, 0x18, 0x00,
0xFF, 0xE7, 0xDB, 0xE7, 0xFF,
0x30, 0x48, 0x3A, 0x06, 0x0E,
0x26, 0x29, 0x79, 0x29, 0x26,
0x40, 0x7F, 0x05, 0x05, 0x07,
0x40, 0x7F, 0x05, 0x25, 0x3F,
0x5A, 0x3C, 0xE7, 0x3C, 0x5A,
0x7F, 0x3E, 0x1C, 0x1C, 0x08,
0x08, 0x1C, 0x1C, 0x3E, 0x7F,
0x14, 0x22, 0x7F, 0x22, 0x14,
0x5F, 0x5F, 0x00, 0x5F, 0x5F,
0x06, 0x09, 0x7F, 0x01, 0x7F,
0x00, 0x66, 0x89, 0x95, 0x6A,
0x60, 0x60, 0x60, 0x60, 0x60,
0x94, 0xA2, 0xFF, 0xA2, 0x94,
0x08, 0x04, 0x7E, 0x04, 0x08,
0x10, 0x20, 0x7E, 0x20, 0x10,
0x08, 0x08, 0x2A, 0x1C, 0x08,
0x08, 0x1C, 0x2A, 0x08, 0x08,
0x1E, 0x10, 0x10, 0x10, 0x10,
0x0C, 0x1E, 0x0C, 0x1E, 0x0C,
0x30, 0x38, 0x3E, 0x38, 0x30,
0x06, 0x0E, 0x3E, 0x0E, 0x06,
0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x5F, 0x00, 0x00,
0x00, 0x07, 0x00, 0x07, 0x00,
0x14, 0x7F, 0x14, 0x7F, 0x14,
0x24, 0x2A, 0x7F, 0x2A, 0x12,
0x23, 0x13, 0x08, 0x64, 0x62,
0x36, 0x49, 0x56, 0x20, 0x50,
0x00, 0x08, 0x07, 0x03, 0x00,
0x00, 0x1C, 0x22, 0x41, 0x00,
0x00, 0x41, 0x22, 0x1C, 0x00,
0x2A, 0x1C, 0x7F, 0x1C, 0x2A,
0x08, 0x08, 0x3E, 0x08, 0x08,
0x00, 0x80, 0x70, 0x30, 0x00,
0x08, 0x08, 0x08, 0x08, 0x08,
0x00, 0x00, 0x60, 0x60, 0x00,
0x20, 0x10, 0x08, 0x04, 0x02,
0x3E, 0x51, 0x49, 0x45, 0x3E,
0x00, 0x42, 0x7F, 0x40, 0x00,
0x72, 0x49, 0x49, 0x49, 0x46,
0x21, 0x41, 0x49, 0x4D, 0x33,
0x18, 0x14, 0x12, 0x7F, 0x10,
0x27, 0x45, 0x45, 0x45, 0x39,
0x3C, 0x4A, 0x49, 0x49, 0x31,
0x41, 0x21, 0x11, 0x09, 0x07,
0x36, 0x49, 0x49, 0x49, 0x36,
0x46, 0x49, 0x49, 0x29, 0x1E,
0x00, 0x00, 0x14, 0x00, 0x00,
0x00, 0x40, 0x34, 0x00, 0x00,
0x00, 0x08, 0x14, 0x22, 0x41,
0x14, 0x14, 0x14, 0x14, 0x14,
0x00, 0x41, 0x22, 0x14, 0x08,
0x02, 0x01, 0x59, 0x09, 0x06,
0x3E, 0x41, 0x5D, 0x59, 0x4E,
0x7C, 0x12, 0x11, 0x12, 0x7C,
0x7F, 0x49, 0x49, 0x49, 0x36,
0x3E, 0x41, 0x41, 0x41, 0x22,
0x7F, 0x41, 0x41, 0x41, 0x3E,
0x7F, 0x49, 0x49, 0x49, 0x41,
0x7F, 0x09, 0x09, 0x09, 0x01,
0x3E, 0x41, 0x41, 0x51, 0x73,
0x7F, 0x08, 0x08, 0x08, 0x7F,
0x00, 0x41, 0x7F, 0x41, 0x00,
0x20, 0x40, 0x41, 0x3F, 0x01,
0x7F, 0x08, 0x14, 0x22, 0x41,
0x7F, 0x40, 0x40, 0x40, 0x40,
0x7F, 0x02, 0x1C, 0x02, 0x7F,
0x7F, 0x04, 0x08, 0x10, 0x7F,
0x3E, 0x41, 0x41, 0x41, 0x3E,
0x7F, 0x09, 0x09, 0x09, 0x06,
0x3E, 0x41, 0x51, 0x21, 0x5E,
0x7F, 0x09, 0x19, 0x29, 0x46,
0x26, 0x49, 0x49, 0x49, 0x32,
0x03, 0x01, 0x7F, 0x01, 0x03,
0x3F, 0x40, 0x40, 0x40, 0x3F,
0x1F, 0x20, 0x40, 0x20, 0x1F,
0x3F, 0x40, 0x38, 0x40, 0x3F,
0x63, 0x14, 0x08, 0x14, 0x63,
0x03, 0x04, 0x78, 0x04, 0x03,
0x61, 0x59, 0x49, 0x4D, 0x43,
0x00, 0x7F, 0x41, 0x41, 0x41,
0x02, 0x04, 0x08, 0x10, 0x20,
0x00, 0x41, 0x41, 0x41, 0x7F,
0x04, 0x02, 0x01, 0x02, 0x04,
0x40, 0x40, 0x40, 0x40, 0x40,
0x00, 0x03, 0x07, 0x08, 0x00,
0x20, 0x54, 0x54, 0x78, 0x40,
0x7F, 0x28, 0x44, 0x44, 0x38,
0x38, 0x44, 0x44, 0x44, 0x28,
0x38, 0x44, 0x44, 0x28, 0x7F,
0x38, 0x54, 0x54, 0x54, 0x18,
0x00, 0x08, 0x7E, 0x09, 0x02,
0x18, 0xA4, 0xA4, 0x9C, 0x78,
0x7F, 0x08, 0x04, 0x04, 0x78,
0x00, 0x44, 0x7D, 0x40, 0x00,
0x20, 0x40, 0x40, 0x3D, 0x00,
0x7F, 0x10, 0x28, 0x44, 0x00,
0x00, 0x41, 0x7F, 0x40, 0x00,
0x7C, 0x04, 0x78, 0x04, 0x78,
0x7C, 0x08, 0x04, 0x04, 0x78,
0x38, 0x44, 0x44, 0x44, 0x38,
0xFC, 0x18, 0x24, 0x24, 0x18,
0x18, 0x24, 0x24, 0x18, 0xFC,
0x7C, 0x08, 0x04, 0x04, 0x08,
0x48, 0x54, 0x54, 0x54, 0x24,
0x04, 0x04, 0x3F, 0x44, 0x24,
0x3C, 0x40, 0x40, 0x20, 0x7C,
0x1C, 0x20, 0x40, 0x20, 0x1C,
0x3C, 0x40, 0x30, 0x40, 0x3C,
0x44, 0x28, 0x10, 0x28, 0x44,
0x4C, 0x90, 0x90, 0x90, 0x7C,
0x44, 0x64, 0x54, 0x4C, 0x44,
0x00, 0x08, 0x36, 0x41, 0x00,
0x00, 0x00, 0x77, 0x00, 0x00,
0x00, 0x41, 0x36, 0x08, 0x00,
0x02, 0x01, 0x02, 0x04, 0x02,
0x3C, 0x26, 0x23, 0x26, 0x3C,
0x1E, 0xA1, 0xA1, 0x61, 0x12,
0x3A, 0x40, 0x40, 0x20, 0x7A,
0x38, 0x54, 0x54, 0x55, 0x59,
0x21, 0x55, 0x55, 0x79, 0x41,
0x22, 0x54, 0x54, 0x78, 0x42,
0x21, 0x55, 0x54, 0x78, 0x40,
0x20, 0x54, 0x55, 0x79, 0x40,
0x0C, 0x1E, 0x52, 0x72, 0x12,
0x39, 0x55, 0x55, 0x55, 0x59,
0x39, 0x54, 0x54, 0x54, 0x59,
0x39, 0x55, 0x54, 0x54, 0x58,
0x00, 0x00, 0x45, 0x7C, 0x41,
0x00, 0x02, 0x45, 0x7D, 0x42,
0x00, 0x01, 0x45, 0x7C, 0x40,
0x7D, 0x12, 0x11, 0x12, 0x7D,
0xF0, 0x28, 0x25, 0x28, 0xF0,
0x7C, 0x54, 0x55, 0x45, 0x00,
0x20, 0x54, 0x54, 0x7C, 0x54,
0x7C, 0x0A, 0x09, 0x7F, 0x49,
0x32, 0x49, 0x49, 0x49, 0x32,
0x3A, 0x44, 0x44, 0x44, 0x3A,
0x32, 0x4A, 0x48, 0x48, 0x30,
0x3A, 0x41, 0x41, 0x21, 0x7A,
0x3A, 0x42, 0x40, 0x20, 0x78,
0x00, 0x9D, 0xA0, 0xA0, 0x7D,
0x3D, 0x42, 0x42, 0x42, 0x3D,
0x3D, 0x40, 0x40, 0x40, 0x3D,
0x3C, 0x24, 0xFF, 0x24, 0x24,
0x48, 0x7E, 0x49, 0x43, 0x66,
0x2B, 0x2F, 0xFC, 0x2F, 0x2B,
0xFF, 0x09, 0x29, 0xF6, 0x20,
0xC0, 0x88, 0x7E, 0x09, 0x03,
0x20, 0x54, 0x54, 0x79, 0x41,
0x00, 0x00, 0x44, 0x7D, 0x41,
0x30, 0x48, 0x48, 0x4A, 0x32,
0x38, 0x40, 0x40, 0x22, 0x7A,
0x00, 0x7A, 0x0A, 0x0A, 0x72,
0x7D, 0x0D, 0x19, 0x31, 0x7D,
0x26, 0x29, 0x29, 0x2F, 0x28,
0x26, 0x29, 0x29, 0x29, 0x26,
0x30, 0x48, 0x4D, 0x40, 0x20,
0x38, 0x08, 0x08, 0x08, 0x08,
0x08, 0x08, 0x08, 0x08, 0x38,
0x2F, 0x10, 0xC8, 0xAC, 0xBA,
0x2F, 0x10, 0x28, 0x34, 0xFA,
0x00, 0x00, 0x7B, 0x00, 0x00,
0x08, 0x14, 0x2A, 0x14, 0x22,
0x22, 0x14, 0x2A, 0x14, 0x08,
0x55, 0x00, 0x55, 0x00, 0x55,
0xAA, 0x55, 0xAA, 0x55, 0xAA,
0xFF, 0x55, 0xFF, 0x55, 0xFF,
0x00, 0x00, 0x00, 0xFF, 0x00,
0x10, 0x10, 0x10, 0xFF, 0x00,
0x14, 0x14, 0x14, 0xFF, 0x00,
0x10, 0x10, 0xFF, 0x00, 0xFF,
0x10, 0x10, 0xF0, 0x10, 0xF0,
0x14, 0x14, 0x14, 0xFC, 0x00,
0x14, 0x14, 0xF7, 0x00, 0xFF,
0x00, 0x00, 0xFF, 0x00, 0xFF,
0x14, 0x14, 0xF4, 0x04, 0xFC,
0x14, 0x14, 0x17, 0x10, 0x1F,
0x10, 0x10, 0x1F, 0x10, 0x1F,
0x14, 0x14, 0x14, 0x1F, 0x00,
0x10, 0x10, 0x10, 0xF0, 0x00,
0x00, 0x00, 0x00, 0x1F, 0x10,
0x10, 0x10, 0x10, 0x1F, 0x10,
0x10, 0x10, 0x10, 0xF0, 0x10,
0x00, 0x00, 0x00, 0xFF, 0x10,
0x10, 0x10, 0x10, 0x10, 0x10,
0x10, 0x10, 0x10, 0xFF, 0x10,
0x00, 0x00, 0x00, 0xFF, 0x14,
0x00, 0x00, 0xFF, 0x00, 0xFF,
0x00, 0x00, 0x1F, 0x10, 0x17,
0x00, 0x00, 0xFC, 0x04, 0xF4,
0x14, 0x14, 0x17, 0x10, 0x17,
0x14, 0x14, 0xF4, 0x04, 0xF4,
0x00, 0x00, 0xFF, 0x00, 0xF7,
0x14, 0x14, 0x14, 0x14, 0x14,
0x14, 0x14, 0xF7, 0x00, 0xF7,
0x14, 0x14, 0x14, 0x17, 0x14,
0x10, 0x10, 0x1F, 0x10, 0x1F,
0x14, 0x14, 0x14, 0xF4, 0x14,
0x10, 0x10, 0xF0, 0x10, 0xF0,
0x00, 0x00, 0x1F, 0x10, 0x1F,
0x00, 0x00, 0x00, 0x1F, 0x14,
0x00, 0x00, 0x00, 0xFC, 0x14,
0x00, 0x00, 0xF0, 0x10, 0xF0,
0x10, 0x10, 0xFF, 0x10, 0xFF,
0x14, 0x14, 0x14, 0xFF, 0x14,
0x10, 0x10, 0x10, 0x1F, 0x00,
0x00, 0x00, 0x00, 0xF0, 0x10,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xF0, 0xF0, 0xF0, 0xF0, 0xF0,
0xFF, 0xFF, 0xFF, 0x00, 0x00,
0x00, 0x00, 0x00, 0xFF, 0xFF,
0x0F, 0x0F, 0x0F, 0x0F, 0x0F,
0x38, 0x44, 0x44, 0x38, 0x44,
0xFC, 0x4A, 0x4A, 0x4A, 0x34,
0x7E, 0x02, 0x02, 0x06, 0x06,
0x02, 0x7E, 0x02, 0x7E, 0x02,
0x63, 0x55, 0x49, 0x41, 0x63,
0x38, 0x44, 0x44, 0x3C, 0x04,
0x40, 0x7E, 0x20, 0x1E, 0x20,
0x06, 0x02, 0x7E, 0x02, 0x02,
0x99, 0xA5, 0xE7, 0xA5, 0x99,
0x1C, 0x2A, 0x49, 0x2A, 0x1C,
0x4C, 0x72, 0x01, 0x72, 0x4C,
0x30, 0x4A, 0x4D, 0x4D, 0x30,
0x30, 0x48, 0x78, 0x48, 0x30,
0xBC, 0x62, 0x5A, 0x46, 0x3D,
0x3E, 0x49, 0x49, 0x49, 0x00,
0x7E, 0x01, 0x01, 0x01, 0x7E,
0x2A, 0x2A, 0x2A, 0x2A, 0x2A,
0x44, 0x44, 0x5F, 0x44, 0x44,
0x40, 0x51, 0x4A, 0x44, 0x40,
0x40, 0x44, 0x4A, 0x51, 0x40,
0x00, 0x00, 0xFF, 0x01, 0x03,
0xE0, 0x80, 0xFF, 0x00, 0x00,
0x08, 0x08, 0x6B, 0x6B, 0x08,
0x36, 0x12, 0x36, 0x24, 0x36,
0x06, 0x0F, 0x09, 0x0F, 0x06,
0x00, 0x00, 0x18, 0x18, 0x00,
0x00, 0x00, 0x10, 0x10, 0x00,
0x30, 0x40, 0xFF, 0x01, 0x01,
0x00, 0x1F, 0x01, 0x01, 0x1E,
0x00, 0x19, 0x1D, 0x17, 0x12,
0x00, 0x3C, 0x3C, 0x3C, 0x3C,
0x00, 0x00, 0x00, 0x00, 0x00
};
bool Display::init(void)
{
memset(&_flags, 0, sizeof(_flags));
_healthy = hw_init();
if (!_healthy) {
return false;
}
// update all on display
update_all();
hw_update();
return true;
}
void Display::update()
{
static uint8_t timer = 0;
// return immediately if not enabled
if (!_healthy) {
return;
}
// max update frequency 2Hz
if (timer++ < 25) {
return;
}
timer = 0;
// check if status has changed
if (_flags.armed != AP_Notify::flags.armed) {
update_arm();
_flags.armed = AP_Notify::flags.armed;
}
if (_flags.pre_arm_check != AP_Notify::flags.pre_arm_check) {
update_prearm();
_flags.pre_arm_check = AP_Notify::flags.pre_arm_check;
}
if (_flags.gps_status != AP_Notify::flags.gps_status) {
update_gps();
_flags.gps_status = AP_Notify::flags.gps_status;
}
if (_flags.gps_num_sats != AP_Notify::flags.gps_num_sats) {
update_gps_sats();
_flags.gps_num_sats = AP_Notify::flags.gps_num_sats;
}
if (_flags.ekf_bad != AP_Notify::flags.ekf_bad) {
update_ekf();
_flags.ekf_bad = AP_Notify::flags.ekf_bad;
}
// if somethings has changed, update display
if (_need_update) {
hw_update();
_need_update = false;
}
}
void Display::update_all()
{
update_arm();
update_prearm();
update_gps();
update_gps_sats();
update_ekf();
}
void Display::draw_text(uint16_t x, uint16_t y, const char* c)
{
while (*c != 0) {
draw_char(x, y, *c);
x += 7;
c++;
}
}
void Display::draw_char(uint16_t x, uint16_t y, const char c)
{
uint8_t line;
// draw char to pixel
for (uint8_t i = 0; i < 6; i++) {
if (i == 5) {
line = 0;
} else {
line = _font[(c * 5) + i];
}
for (uint8_t j = 0; j < 8; j++) {
if (line & 1) {
set_pixel(x + i, y + j);
} else {
clear_pixel(x + i, y + j);
}
line >>= 1;
}
}
// update display
_need_update = true;
}
void Display::update_arm()
{
if (AP_Notify::flags.armed) {
draw_text(COLUMN(0), ROW(0), ">>>>> ARMED! <<<<<");
} else {
draw_text(COLUMN(0), ROW(0), " disarmed ");
}
}
void Display::update_prearm()
{
if (AP_Notify::flags.pre_arm_check) {
draw_text(COLUMN(0), ROW(2), "Prearm: passed ");
} else {
draw_text(COLUMN(0), ROW(2), "Prearm: failed ");
}
}
void Display::update_gps()
{
switch (AP_Notify::flags.gps_status) {
case AP_GPS::NO_GPS:
draw_text(COLUMN(0), ROW(3), "GPS: no GPS");
break;
case AP_GPS::NO_FIX:
draw_text(COLUMN(0), ROW(3), "GPS: no fix");
break;
case AP_GPS::GPS_OK_FIX_2D:
draw_text(COLUMN(0), ROW(3), "GPS: 2D ");
break;
case AP_GPS::GPS_OK_FIX_3D:
draw_text(COLUMN(0), ROW(3), "GPS: 3D ");
break;
case AP_GPS::GPS_OK_FIX_3D_DGPS:
draw_text(COLUMN(0), ROW(3), "GPS: DGPS ");
break;
case AP_GPS::GPS_OK_FIX_3D_RTK:
draw_text(COLUMN(0), ROW(3), "GPS: RTK ");
break;
}
}
void Display::update_gps_sats()
{
draw_text(COLUMN(0), ROW(4), "Sats:");
draw_char(COLUMN(8), ROW(4), (AP_Notify::flags.gps_num_sats / 10) + 48);
draw_char(COLUMN(9), ROW(4), (AP_Notify::flags.gps_num_sats % 10) + 48);
}
void Display::update_ekf()
{
if (AP_Notify::flags.ekf_bad) {
draw_text(COLUMN(0), ROW(5), "EKF: fail");
} else {
draw_text(COLUMN(0), ROW(5), "EKF: ok ");
}
}

42
libraries/AP_Notify/Display.h Normal file
View File

@ -0,0 +1,42 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#include "NotifyDevice.h"
#define ROW(Y) ((Y * 11) + 2)
#define COLUMN(X) ((X * 7) + 1)
class Display: public NotifyDevice {
public:
bool init(void);
void update();
protected:
virtual bool hw_init() = 0;
virtual bool hw_update() = 0;
virtual bool set_pixel(uint16_t x, uint16_t y) = 0;
virtual bool clear_pixel(uint16_t x, uint16_t y) = 0;
private:
void draw_char(uint16_t x, uint16_t y, const char c);
void draw_text(uint16_t x, uint16_t y, const char *c);
void update_all();
void update_arm();
void update_prearm();
void update_gps();
void update_gps_sats();
void update_ekf();
bool _healthy;
bool _need_update;
struct display_state {
uint32_t gps_status : 3; // 0 = no gps, 1 = no lock, 2 = 2d lock, 3 = 3d lock, 4 = dgps lock, 5 = rtk lock
uint32_t gps_num_sats : 6; // number of sats
uint32_t armed : 1; // 0 = disarmed, 1 = armed
uint32_t pre_arm_check : 1; // 0 = failing checks, 1 = passed
uint32_t ekf_bad : 1; // 1 if ekf is reporting problems
} _flags;
};

107
libraries/AP_Notify/Display_SSD1306_I2C.cpp Normal file
View File

@ -0,0 +1,107 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "Display_SSD1306_I2C.h"
static const AP_HAL::HAL& hal = AP_HAL::get_HAL();
bool Display_SSD1306_I2C::hw_init()
{
// display init sequence
uint8_t init_seq[] = {0xAE, 0xD5, 0x80, 0xA8, 0x3F, 0xD3,
0x00, 0x40, 0x8D, 0x14, 0x20, 0x00,
0xA1, 0xC8, 0xDA, 0x12, 0x81, 0xCF,
0xD9, 0xF1, 0xDB, 0x40, 0xA4, 0xA6,
0xAF, 0x21, 0, 127, 0x22, 0, 7
};
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
// disable recording of i2c lockup errors
hal.i2c->ignore_errors(true);
// init display
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(init_seq), init_seq);
// re-enable recording of i2c lockup errors
hal.i2c->ignore_errors(false);
// give back i2c semaphore
i2c_sem->give();
// clear display
hw_update();
return true;
}
bool Display_SSD1306_I2C::hw_update()
{
uint8_t command[] = {0x21, 0, 127, 0x22, 0, 7};
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(5)) {
return false;
}
// write buffer to display
for (uint8_t i = 0; i < (SSD1306_COLUMNS / SSD1306_COLUMNS_PER_PAGE); i++) {
command[4] = i;
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x0, sizeof(command), command);
hal.i2c->writeRegisters(SSD1306_I2C_ADDRESS, 0x40, SSD1306_ROWS, &_displaybuffer[i * SSD1306_ROWS]);
}
// give back i2c semaphore
i2c_sem->give();
return true;
}
bool Display_SSD1306_I2C::set_pixel(uint16_t x, uint16_t y)
{
// check x, y range
if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) {
return false;
}
// set pixel in buffer
_displaybuffer[x + (y / 8 * SSD1306_ROWS)] |= 1 << (y % 8);
return true;
}
bool Display_SSD1306_I2C::clear_pixel(uint16_t x, uint16_t y)
{
// check x, y range
if ((x >= SSD1306_ROWS) || (y >= SSD1306_COLUMNS)) {
return false;
}
// clear pixel in buffer
_displaybuffer[x + (y / 8 * SSD1306_ROWS)] &= ~(1 << (y % 8));
return true;
}

21
libraries/AP_Notify/Display_SSD1306_I2C.h Normal file
View File

@ -0,0 +1,21 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#pragma once
#include "Display.h"
#define SSD1306_I2C_ADDRESS 0x3C // default I2C bus address
#define SSD1306_ROWS 128 // display rows
#define SSD1306_COLUMNS 64 // display columns
#define SSD1306_COLUMNS_PER_PAGE 8
class Display_SSD1306_I2C: public Display {
public:
virtual bool hw_init();
virtual bool hw_update();
virtual bool set_pixel(uint16_t x, uint16_t y);
virtual bool clear_pixel(uint16_t x, uint16_t y);
private:
uint8_t _displaybuffer[SSD1306_ROWS * SSD1306_COLUMNS_PER_PAGE];
};

34
libraries/AP_Param/AP_Param.cpp
View File

@ -67,6 +67,9 @@ extern const AP_HAL::HAL &hal;
// number of rows in the _var_info[] table
uint16_t AP_Param::_num_vars;
// cached parameter count
uint16_t AP_Param::_parameter_count;
// storage and naming information about all types that can be saved
const AP_Param::Info *AP_Param::_var_info;
@ -823,6 +826,11 @@ bool AP_Param::save(bool force_save)
ap = (const AP_Param *)((ptrdiff_t)ap) - (idx*sizeof(float));
}
if (phdr.type == AP_PARAM_INT8 && ginfo != nullptr && (ginfo->flags & AP_PARAM_FLAG_ENABLE)) {
// clear cached parameter count
_parameter_count = 0;
}
char name[AP_MAX_NAME_SIZE+1];
copy_name_info(info, ginfo, ginfo0, idx, name, sizeof(name), true);
@ -1069,12 +1077,12 @@ bool AP_Param::load_all(void)
struct Param_header phdr;
uint16_t ofs = sizeof(AP_Param::EEPROM_header);
#if HAL_OS_POSIX_IO == 1
/*
if the HAL specifies a defaults parameter file then override
defaults using that file
*/
#ifdef HAL_PARAM_DEFAULTS_PATH
load_defaults_file(HAL_PARAM_DEFAULTS_PATH);
load_defaults_file(hal.util->get_custom_defaults_file());
#endif
while (ofs < _storage.size()) {
@ -1545,6 +1553,9 @@ bool AP_Param::parse_param_line(char *line, char **vname, float &value)
*/
bool AP_Param::load_defaults_file(const char *filename)
{
if (filename == nullptr) {
return false;
}
FILE *f = fopen(filename, "r");
if (f == NULL) {
return false;
@ -1656,3 +1667,22 @@ void AP_Param::send_parameter(char *name, enum ap_var_type param_header_type) co
name_axis = 'Z';
GCS_MAVLINK::send_parameter_value_all(name, AP_PARAM_FLOAT, v->z);
}
/*
return count of all scalar parameters
*/
uint16_t AP_Param::count_parameters(void)
{
// if we haven't cached the parameter count yet...
if (0 == _parameter_count) {
AP_Param *vp;
AP_Param::ParamToken token;
vp = AP_Param::first(&token, NULL);
do {
_parameter_count++;
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
}
return _parameter_count;
}

4
libraries/AP_Param/AP_Param.h
View File

@ -313,6 +313,9 @@ public:
// return true if the parameter is configured
bool configured(void) { return configured_in_defaults_file() || configured_in_storage(); }
// count of parameters in tree
static uint16_t count_parameters(void);
private:
/// EEPROM header
///
@ -439,6 +442,7 @@ private:
static StorageAccess _storage;
static uint16_t _num_vars;
static uint16_t _parameter_count;
static const struct Info * _var_info;
/*

10
libraries/AP_SpdHgtControl/AP_SpdHgtControl.h
View File

@ -31,9 +31,10 @@ public:
enum FlightStage {
FLIGHT_NORMAL = 1,
FLIGHT_TAKEOFF = 2,
FLIGHT_LAND_APPROACH = 3,
FLIGHT_LAND_FINAL = 4,
FLIGHT_LAND_ABORT = 5
FLIGHT_VTOL = 3,
FLIGHT_LAND_APPROACH = 4,
FLIGHT_LAND_FINAL = 5,
FLIGHT_LAND_ABORT = 6
};
// Update of the pitch and throttle demands
@ -69,6 +70,9 @@ public:
// return landing sink rate
virtual float get_land_sinkrate(void) const = 0;
// set path_proportion accessor
virtual void set_path_proportion(float path_proportion) = 0;
// add new controllers to this enum. Users can then
// select which controller to use by setting the
// SPDHGT_CONTROLLER parameter

14
libraries/AP_TECS/AP_TECS.cpp
View File

@ -122,7 +122,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @Param: LAND_THR
// @DisplayName: Cruise throttle during landing approach (percentage)
// @Description: Use this parameter instead of LAND_ASPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If it is negative if TECS_LAND_ASPD is in use then this value is not used during landing.
// @Description: Use this parameter instead of LAND_ARSPD if your platform does not have an airspeed sensor. It is the cruise throttle during landing approach. If this value is negative then it is disabled and TECS_LAND_ARSPD is used instead.
// @Range: -1 100
// @Increment: 0.1
// @User: User
@ -454,7 +454,9 @@ void AP_TECS::_update_height_demand(void)
void AP_TECS::_detect_underspeed(void)
{
if (((_integ5_state < _TASmin * 0.9f) &&
if (_flight_stage == AP_TECS::FLIGHT_VTOL) {
_underspeed = false;
} else if (((_integ5_state < _TASmin * 0.9f) &&
(_throttle_dem >= _THRmaxf * 0.95f) &&
_flight_stage != AP_TECS::FLIGHT_LAND_FINAL) ||
((_height < _hgt_dem_adj) && _underspeed))
@ -667,7 +669,13 @@ void AP_TECS::_update_pitch(void)
} else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) {
SKE_weighting = 2.0f;
} else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
if (_spdWeightLand < 0) {
// use sliding scale from normal weight down to zero at landing
float scaled_weight = _spdWeight * (1.0f - _path_proportion);
SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f);
} else {
SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f);
}
}
float SPE_weighting = 2.0f - SKE_weighting;

13
libraries/AP_TECS/AP_TECS.h
View File

@ -87,6 +87,16 @@ public:
return _land_sink;
}
// return height rate demand, in m/s
float get_height_rate_demand(void) const {
return _hgt_rate_dem;
}
// set path_proportion
void set_path_proportion(float path_proportion) {
_path_proportion = constrain_float(path_proportion, 0.0f, 1.0f);
}
// this supports the TECS_* user settable parameters
static const struct AP_Param::GroupInfo var_info[];
@ -266,6 +276,9 @@ private:
// counter for demanded sink rate on land final
uint8_t _flare_counter;
// percent traveled along the previous and next waypoints
float _path_proportion;
// Update the airspeed internal state using a second order complementary filter
void _update_speed(float load_factor);

4
libraries/DataFlash/DFMessageWriter.cpp
View File

@ -37,9 +37,6 @@ void DFMessageWriter_DFLogStart::process()
if (!_dataflash_backend->Log_Write_Format(_dataflash_backend->structure(next_format_to_send))) {
return; // call me again!
}
// provide hook to avoid corrupting the APM1/APM2
// dataflash by writing too fast:
_dataflash_backend->WroteStartupFormat();
next_format_to_send++;
}
_fmt_done = true;
@ -52,7 +49,6 @@ void DFMessageWriter_DFLogStart::process()
return;
}
ap = AP_Param::next_scalar(&token, &type);
_dataflash_backend->WroteStartupParam();
}
stage = ls_blockwriter_stage_sysinfo;

12
libraries/DataFlash/DataFlash.cpp
View File

@ -111,13 +111,6 @@ uint16_t DataFlash_Class::get_num_logs(void) {
}
return backends[0]->get_num_logs();
}
void DataFlash_Class::Log_Fill_Format(const struct LogStructure *s, struct log_Format &pkt) {
if (_next_backend == 0) {
// how were we called?!
return;
}
backends[0]->Log_Fill_Format(s, pkt);
}
void DataFlash_Class::LogReadProcess(uint16_t log_num,
uint16_t start_page, uint16_t end_page,
@ -189,11 +182,6 @@ void DataFlash_Class::Log_Write_EntireMission(const AP_Mission &mission)
FOR_EACH_BACKEND(Log_Write_EntireMission(mission));
}
void DataFlash_Class::Log_Write_Format(const struct LogStructure *s)
{
FOR_EACH_BACKEND(Log_Write_Format(s));
}
void DataFlash_Class::Log_Write_Message(const char *message)
{
FOR_EACH_BACKEND(Log_Write_Message(message));

11
libraries/DataFlash/DataFlash.h
View File

@ -88,7 +88,7 @@ public:
void StartNewLog(void);
void EnableWrites(bool enable);
void Log_Write_Format(const struct LogStructure *structure);
void Log_Write_Parameter(const char *name, float value);
void Log_Write_GPS(const AP_GPS &gps, uint8_t instance, int32_t relative_alt);
void Log_Write_RFND(const RangeFinder &rangefinder);
@ -149,11 +149,6 @@ public:
void periodic_tasks(); // may want to split this into GCS/non-GCS duties
// this is out here for the trickle-startup-messages logging.
// Think before calling.
bool Log_Write_Parameter(const AP_Param *ap, const AP_Param::ParamToken &token,
enum ap_var_type type);
vehicle_startup_message_Log_Writer _vehicle_messages;
// parameter support
@ -166,10 +161,6 @@ public:
const struct LogStructure *structure(uint16_t num) const;
protected:
void Log_Fill_Format(const struct LogStructure *structure, struct log_Format &pkt);
void WroteStartupFormat();
void WroteStartupParam();
const struct LogStructure *_structures;
uint8_t _num_types;

6
libraries/DataFlash/DataFlash_Backend.h
View File

@ -55,9 +55,6 @@ public:
void EnableWrites(bool enable) { _writes_enabled = enable; }
bool logging_started(void) const { return log_write_started; }
// initialisation this really shouldn't take structure and
// num_types, however the CLI LogReadProcess function requires it.
// That function needs to be split.
virtual void Init() {
_writes_enabled = true;
}
@ -86,9 +83,6 @@ public:
uint8_t num_types() const;
const struct LogStructure *structure(uint8_t structure) const;
virtual void WroteStartupFormat() { }
virtual void WroteStartupParam() { }
void Log_Write_EntireMission(const AP_Mission &mission);
bool Log_Write_Format(const struct LogStructure *structure);
bool Log_Write_MavCmd(uint16_t cmd_total, const mavlink_mission_item_t& mav_cmd);

6
libraries/GCS_MAVLink/GCS.h
View File

@ -212,12 +212,6 @@ private:
///
/// @return The number of reportable parameters.
///
static uint16_t _count_parameters(); ///< count reportable
// parameters
static uint16_t _parameter_count; ///< cache of reportable
// parameters
mavlink_channel_t chan;
uint16_t packet_drops;

29
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -27,7 +27,6 @@ extern const AP_HAL::HAL& hal;
uint32_t GCS_MAVLINK::last_radio_status_remrssi_ms;
uint8_t GCS_MAVLINK::mavlink_active = 0;
uint16_t GCS_MAVLINK::_parameter_count;
GCS_MAVLINK::GCS_MAVLINK() :
waypoint_receive_timeout(5000)
@ -103,22 +102,6 @@ GCS_MAVLINK::setup_uart(const AP_SerialManager& serial_manager, AP_SerialManager
init(uart, mav_chan);
}
uint16_t
GCS_MAVLINK::_count_parameters()
{
// if we haven't cached the parameter count yet...
if (0 == _parameter_count) {
AP_Param *vp;
AP_Param::ParamToken token;
vp = AP_Param::first(&token, NULL);
do {
_parameter_count++;
} while (NULL != (vp = AP_Param::next_scalar(&token, NULL)));
}
return _parameter_count;
}
/**
* @brief Send the next pending parameter, called from deferred message
* handling code
@ -520,7 +503,7 @@ void GCS_MAVLINK::handle_param_request_list(mavlink_message_t *msg)
// Start sending parameters - next call to ::update will kick the first one out
_queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type);
_queued_parameter_index = 0;
_queued_parameter_count = _count_parameters();
_queued_parameter_count = AP_Param::count_parameters();
}
void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
@ -555,7 +538,7 @@ void GCS_MAVLINK::handle_param_request_read(mavlink_message_t *msg)
param_name,
value,
mav_var_type(p_type),
_count_parameters(),
AP_Param::count_parameters(),
packet.param_index);
}
@ -606,7 +589,9 @@ GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)
comm_get_txspace(chan) >=
MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_STATUSTEXT_LEN) {
// send immediately
mavlink_msg_statustext_send(chan, severity, str);
char msg[50] {};
strncpy(msg, str, sizeof(msg));
mavlink_msg_statustext_send(chan, severity, msg);
} else {
// send via the deferred queuing system
mavlink_statustext_t *s = &pending_status;
@ -1157,7 +1142,7 @@ void GCS_MAVLINK::send_statustext_all(MAV_SEVERITY severity, const char *fmt, ..
if ((1U<<i) & mavlink_active) {
mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
char msg2[50];
char msg2[50] {};
va_list arg_list;
va_start(arg_list, fmt);
hal.util->vsnprintf((char *)msg2, sizeof(msg2), fmt, arg_list);
@ -1184,7 +1169,7 @@ void GCS_MAVLINK::send_parameter_value_all(const char *param_name, ap_var_type p
param_name,
param_value,
mav_var_type(param_type),
_count_parameters(),
AP_Param::count_parameters(),
-1);
}
}

3
libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/ardupilotmega.h
View File

@ -52,6 +52,8 @@ typedef enum MAV_CMD
MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */
MAV_CMD_NAV_SPLINE_WAYPOINT=82, /* Navigate to MISSION using a spline path. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Empty| Empty| Empty| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */
MAV_CMD_NAV_ALTITUDE_WAIT=83, /* Mission command to wait for an altitude or downwards vertical speed. This is meant for high altitude balloon launches, allowing the aircraft to be idle until either an altitude is reached or a negative vertical speed is reached (indicating early balloon burst). The wiggle time is how often to wiggle the control surfaces to prevent them seizing up. |altitude (m)| descent speed (m/s)| Wiggle Time (s)| Empty| Empty| Empty| Empty| */
MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode |Empty| Empty| Empty| Yaw angle in degrees| Latitude| Longitude| Altitude| */
MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Empty| Yaw angle in degrees| Latitude| Longitude| Altitude| */
MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */
MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */
MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */
@ -104,6 +106,7 @@ typedef enum MAV_CMD
MAV_CMD_VIDEO_START_CAPTURE=2500, /* Starts video capture |Camera ID (0 for all cameras), 1 for first, 2 for second, etc.| Frames per second| Resolution in megapixels (0.3 for 640x480, 1.3 for 1280x720, etc)| */
MAV_CMD_VIDEO_STOP_CAPTURE=2501, /* Stop the current video capture |Reserved| Reserved| */
MAV_CMD_PANORAMA_CREATE=2800, /* Create a panorama at the current position |Viewing angle horizontal of the panorama (in degrees, +- 0.5 the total angle)| Viewing angle vertical of panorama (in degrees)| Speed of the horizontal rotation (in degrees per second)| Speed of the vertical rotation (in degrees per second)| */
MAV_CMD_DO_VTOL_TRANSITION=3000, /* Request VTOL transition |The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.| */
MAV_CMD_PAYLOAD_PREPARE_DEPLOY=30001, /* Deploy payload on a Lat / Lon / Alt position. This includes the navigation to reach the required release position and velocity. |Operation mode. 0: prepare single payload deploy (overwriting previous requests), but do not execute it. 1: execute payload deploy immediately (rejecting further deploy commands during execution, but allowing abort). 2: add payload deploy to existing deployment list.| Desired approach vector in degrees compass heading (0..360). A negative value indicates the system can define the approach vector at will.| Desired ground speed at release time. This can be overriden by the airframe in case it needs to meet minimum airspeed. A negative value indicates the system can define the ground speed at will.| Minimum altitude clearance to the release position in meters. A negative value indicates the system can define the clearance at will.| Latitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Longitude unscaled for MISSION_ITEM or in 1e7 degrees for MISSION_ITEM_INT| Altitude, in meters AMSL| */
MAV_CMD_PAYLOAD_CONTROL_DEPLOY=30002, /* Control the payload deployment. |Operation mode. 0: Abort deployment, continue normal mission. 1: switch to payload deploment mode. 100: delete first payload deployment request. 101: delete all payload deployment requests.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */
MAV_CMD_POWER_OFF_INITIATED=42000, /* A system wide power-off event has been initiated. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */

2
libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Jan 2 08:44:07 2016"
#define MAVLINK_BUILD_DATE "Sat Jan 2 10:50:50 2016"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255

26
libraries/GCS_MAVLink/include/mavlink/v1.0/common/common.h
View File

@ -589,6 +589,32 @@ typedef enum MAV_BATTERY_FUNCTION
} MAV_BATTERY_FUNCTION;
#endif
/** @brief Enumeration of VTOL states */
#ifndef HAVE_ENUM_MAV_VTOL_STATE
#define HAVE_ENUM_MAV_VTOL_STATE
typedef enum MAV_VTOL_STATE
{
MAV_VTOL_STATE_UNDEFINED=0, /* MAV is not configured as VTOL | */
MAV_VTOL_STATE_TRANSITION_TO_FW=1, /* VTOL is in transition from multicopter to fixed-wing | */
MAV_VTOL_STATE_TRANSITION_TO_MC=2, /* VTOL is in transition from fixed-wing to multicopter | */
MAV_VTOL_STATE_MC=3, /* VTOL is in multicopter state | */
MAV_VTOL_STATE_FW=4, /* VTOL is in fixed-wing state | */
MAV_VTOL_STATE_ENUM_END=5, /* | */
} MAV_VTOL_STATE;
#endif
/** @brief Enumeration of landed detector states */
#ifndef HAVE_ENUM_MAV_LANDED_STATE
#define HAVE_ENUM_MAV_LANDED_STATE
typedef enum MAV_LANDED_STATE
{
MAV_LANDED_STATE_UNDEFINED=0, /* MAV landed state is unknown | */
MAV_LANDED_STATE_ON_GROUND=1, /* MAV is landed (on ground) | */
MAV_LANDED_STATE_IN_AIR=2, /* MAV is in air | */
MAV_LANDED_STATE_ENUM_END=3, /* | */
} MAV_LANDED_STATE;
#endif
/** @brief Enumeration of the ADSB altimeter types */
#ifndef HAVE_ENUM_ADSB_ALTITUDE_TYPE
#define HAVE_ENUM_ADSB_ALTITUDE_TYPE

2
libraries/GCS_MAVLink/include/mavlink/v1.0/common/version.h
View File

@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Sat Jan 2 08:44:09 2016"
#define MAVLINK_BUILD_DATE "Sat Jan 2 10:50:53 2016"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255

55
libraries/GCS_MAVLink/message_definitions/common.xml
View File

@ -677,6 +677,26 @@
<param index="6">Longitude/Y of goal</param>
<param index="7">Altitude/Z of goal</param>
</entry>
<entry value="84" name="MAV_CMD_NAV_VTOL_TAKEOFF">
<description>Takeoff from ground using VTOL mode</description>
<param index="1">Empty</param>
<param index="2">Empty</param>
<param index="3">Empty</param>
<param index="4">Yaw angle in degrees</param>
<param index="5">Latitude</param>
<param index="6">Longitude</param>
<param index="7">Altitude</param>
</entry>
<entry value="85" name="MAV_CMD_NAV_VTOL_LAND">
<description>Land using VTOL mode</description>
<param index="1">Empty</param>
<param index="2">Empty</param>
<param index="3">Empty</param>
<param index="4">Yaw angle in degrees</param>
<param index="5">Latitude</param>
<param index="6">Longitude</param>
<param index="7">Altitude</param>
</entry>
<!-- IDs 90 and 91 are reserved until the end of 2014,
as they were used in some conflicting proposals
@ -1151,6 +1171,11 @@
<param index="4">Speed of the vertical rotation (in degrees per second)</param>
</entry>
<entry value="3000" name="MAV_CMD_DO_VTOL_TRANSITION">
<description>Request VTOL transition</description>
<param index="1">The target VTOL state, as defined by ENUM MAV_VTOL_STATE. Only MAV_VTOL_STATE_MC and MAV_VTOL_STATE_FW can be used.</param>
</entry>
<!-- VALUES FROM 0-40000 are reserved for the common message set. Values from 40000 to UINT16_MAX are available for dialects -->
<!-- BEGIN of payload range (30000 to 30999) -->
@ -1546,6 +1571,36 @@
<description>Payload battery</description>
</entry>
</enum>
<enum name="MAV_VTOL_STATE">
<description>Enumeration of VTOL states</description>
<entry value="0" name="MAV_VTOL_STATE_UNDEFINED">
<description>MAV is not configured as VTOL</description>
</entry>
<entry value="1" name="MAV_VTOL_STATE_TRANSITION_TO_FW">
<description>VTOL is in transition from multicopter to fixed-wing</description>
</entry>
<entry value="2" name="MAV_VTOL_STATE_TRANSITION_TO_MC">
<description>VTOL is in transition from fixed-wing to multicopter</description>
</entry>
<entry value="3" name="MAV_VTOL_STATE_MC">
<description>VTOL is in multicopter state</description>
</entry>
<entry value="4" name="MAV_VTOL_STATE_FW">
<description>VTOL is in fixed-wing state</description>
</entry>
</enum>
<enum name="MAV_LANDED_STATE">
<description>Enumeration of landed detector states</description>
<entry value="0" name="MAV_LANDED_STATE_UNDEFINED">
<description>MAV landed state is unknown</description>
</entry>
<entry value="1" name="MAV_LANDED_STATE_ON_GROUND">
<description>MAV is landed (on ground)</description>
</entry>
<entry value="2" name="MAV_LANDED_STATE_IN_AIR">
<description>MAV is in air</description>
</entry>
</enum>
<enum name="ADSB_ALTITUDE_TYPE">
<description>Enumeration of the ADSB altimeter types</description>
<entry value="0" name="ADSB_ALTITUDE_TYPE_PRESSURE_QNH">

16
libraries/RC_Channel/RC_Channel.cpp
View File

@ -312,10 +312,10 @@ RC_Channel::update_min_max()
the current radio_in value using the specified dead_zone
*/
int16_t
RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
RC_Channel::pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t _trim)
{
int16_t radio_trim_high = radio_trim + dead_zone;
int16_t radio_trim_low = radio_trim - dead_zone;
int16_t radio_trim_high = _trim + dead_zone;
int16_t radio_trim_low = _trim - dead_zone;
// prevent div by 0
if ((radio_trim_low - radio_min) == 0 || (radio_max - radio_trim_high) == 0)
@ -330,6 +330,16 @@ RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
return 0;
}
/*
return an "angle in centidegrees" (normally -4500 to 4500) from
the current radio_in value using the specified dead_zone
*/
int16_t
RC_Channel::pwm_to_angle_dz(uint16_t dead_zone)
{
return pwm_to_angle_dz_trim(dead_zone, radio_trim);
}
/*
return an "angle in centidegrees" (normally -4500 to 4500) from
the current radio_in value

2
libraries/RC_Channel/RC_Channel.h
View File

@ -62,6 +62,7 @@ public:
void set_reverse(bool reverse);
bool get_reverse(void) const;
void set_default_dead_zone(int16_t dzone);
uint16_t get_dead_zone(void) const { return _dead_zone; }
// get the channel number
uint8_t get_ch_out(void) const { return _ch_out; };
@ -105,6 +106,7 @@ public:
// includes offset from PWM
//int16_t get_radio_out(void);
int16_t pwm_to_angle_dz_trim(uint16_t dead_zone, uint16_t trim);
int16_t pwm_to_angle_dz(uint16_t dead_zone);
int16_t pwm_to_angle();

3
libraries/RC_Channel/RC_Channel_aux.cpp
View File

@ -391,6 +391,9 @@ bool RC_Channel_aux::set_aux_channel_default(RC_Channel_aux::Aux_servo_function_
for (uint8_t i=0; i<RC_AUX_MAX_CHANNELS; i++) {
if (_aux_channels[i] && _aux_channels[i]->_ch_out == channel) {
if (_aux_channels[i]->function != k_none) {
if (_aux_channels[i]->function == function) {
return true;
}
hal.console->printf("Channel %u already assigned %u\n",
(unsigned)channel,
(unsigned)_aux_channels[i]->function);

Some files were not shown because too many files have changed in this diff Show More