mirror of https://github.com/ArduPilot/ardupilot
Plane: convert from .pde to .cpp files
This commit is contained in:
Andrew Tridgell
parent
fb97c16a84
commit
18c37935c9
|
|
@ -0,0 +1,934 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/*
|
||||
Lead developer: Andrew Tridgell
|
||||
|
||||
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
|
||||
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
|
||||
|
||||
Please contribute your ideas! See http://dev.ardupilot.com for details
|
||||
|
||||
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 "Plane.h"
|
||||
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wpmf-conversions"
|
||||
#define SCHED_TASK(func) AP_HAL_CLASSPROC_VOID(&plane, &Plane::func)
|
||||
|
||||
/*
|
||||
scheduler table - all regular tasks are listed here, along with how
|
||||
often they should be called (in 20ms units) and the maximum time
|
||||
they are expected to take (in microseconds)
|
||||
*/
|
||||
const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
|
||||
{ SCHED_TASK(read_radio), 1, 700 }, // 0
|
||||
{ SCHED_TASK(check_short_failsafe), 1, 1000 },
|
||||
{ SCHED_TASK(ahrs_update), 1, 6400 },
|
||||
{ SCHED_TASK(update_speed_height), 1, 1600 },
|
||||
{ SCHED_TASK(update_flight_mode), 1, 1400 },
|
||||
{ SCHED_TASK(stabilize), 1, 3500 },
|
||||
{ SCHED_TASK(set_servos), 1, 1600 },
|
||||
{ SCHED_TASK(read_control_switch), 7, 1000 },
|
||||
{ SCHED_TASK(gcs_retry_deferred), 1, 1000 },
|
||||
{ SCHED_TASK(update_GPS_50Hz), 1, 2500 },
|
||||
{ SCHED_TASK(update_GPS_10Hz), 5, 2500 }, // 10
|
||||
{ SCHED_TASK(navigate), 5, 3000 },
|
||||
{ SCHED_TASK(update_compass), 5, 1200 },
|
||||
{ SCHED_TASK(read_airspeed), 5, 1200 },
|
||||
{ SCHED_TASK(update_alt), 5, 3400 },
|
||||
{ SCHED_TASK(adjust_altitude_target), 5, 1000 },
|
||||
{ SCHED_TASK(obc_fs_check), 5, 1000 },
|
||||
{ SCHED_TASK(gcs_update), 1, 1700 },
|
||||
{ SCHED_TASK(gcs_data_stream_send), 1, 3000 },
|
||||
{ SCHED_TASK(update_events), 1, 1500 }, // 20
|
||||
{ SCHED_TASK(check_usb_mux), 5, 300 },
|
||||
{ SCHED_TASK(read_battery), 5, 1000 },
|
||||
{ SCHED_TASK(compass_accumulate), 1, 1500 },
|
||||
{ SCHED_TASK(barometer_accumulate), 1, 900 },
|
||||
{ SCHED_TASK(update_notify), 1, 300 },
|
||||
{ SCHED_TASK(read_rangefinder), 1, 500 },
|
||||
#if OPTFLOW == ENABLED
|
||||
{ SCHED_TASK(update_optical_flow), 1, 500 },
|
||||
#endif
|
||||
{ SCHED_TASK(one_second_loop), 50, 1000 },
|
||||
{ SCHED_TASK(check_long_failsafe), 15, 1000 },
|
||||
{ SCHED_TASK(read_receiver_rssi), 5, 1000 },
|
||||
{ SCHED_TASK(airspeed_ratio_update), 50, 1000 }, // 30
|
||||
{ SCHED_TASK(update_mount), 1, 1500 },
|
||||
{ SCHED_TASK(log_perf_info), 500, 1000 },
|
||||
{ SCHED_TASK(compass_save), 3000, 2500 },
|
||||
{ SCHED_TASK(update_logging1), 5, 1700 },
|
||||
{ SCHED_TASK(update_logging2), 5, 1700 },
|
||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||
{ SCHED_TASK(frsky_telemetry_send), 10, 100 },
|
||||
#endif
|
||||
{ SCHED_TASK(terrain_update), 5, 500 },
|
||||
};
|
||||
|
||||
#pragma GCC diagnostic pop
|
||||
|
||||
void Plane::setup()
|
||||
{
|
||||
cliSerial = hal.console;
|
||||
|
||||
// load the default values of variables listed in var_info[]
|
||||
AP_Param::setup_sketch_defaults();
|
||||
|
||||
AP_Notify::flags.failsafe_battery = false;
|
||||
|
||||
notify.init(false);
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
|
||||
|
||||
init_ardupilot();
|
||||
|
||||
// initialise the main loop scheduler
|
||||
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]), NULL);
|
||||
}
|
||||
|
||||
void Plane::loop()
|
||||
{
|
||||
// wait for an INS sample
|
||||
ins.wait_for_sample();
|
||||
|
||||
uint32_t timer = hal.scheduler->micros();
|
||||
|
||||
delta_us_fast_loop = timer - fast_loopTimer_us;
|
||||
G_Dt = delta_us_fast_loop * 1.0e-6f;
|
||||
|
||||
if (delta_us_fast_loop > G_Dt_max && fast_loopTimer_us != 0) {
|
||||
G_Dt_max = delta_us_fast_loop;
|
||||
}
|
||||
|
||||
if (delta_us_fast_loop < G_Dt_min || G_Dt_min == 0) {
|
||||
G_Dt_min = delta_us_fast_loop;
|
||||
}
|
||||
fast_loopTimer_us = timer;
|
||||
|
||||
mainLoop_count++;
|
||||
|
||||
// tell the scheduler one tick has passed
|
||||
scheduler.tick();
|
||||
|
||||
// run all the tasks that are due to run. Note that we only
|
||||
// have to call this once per loop, as the tasks are scheduled
|
||||
// in multiples of the main loop tick. So if they don't run on
|
||||
// the first call to the scheduler they won't run on a later
|
||||
// call until scheduler.tick() is called again
|
||||
uint32_t remaining = (timer + 20000) - hal.scheduler->micros();
|
||||
if (remaining > 19500) {
|
||||
remaining = 19500;
|
||||
}
|
||||
scheduler.run(remaining);
|
||||
}
|
||||
|
||||
// update AHRS system
|
||||
void Plane::ahrs_update()
|
||||
{
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
||||
|
||||
if (g.hil_mode == 1) {
|
||||
// update hil before AHRS update
|
||||
gcs_update();
|
||||
}
|
||||
|
||||
ahrs.update();
|
||||
|
||||
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
|
||||
if (should_log(MASK_LOG_IMU))
|
||||
Log_Write_IMU();
|
||||
|
||||
// calculate a scaled roll limit based on current pitch
|
||||
roll_limit_cd = g.roll_limit_cd * cosf(ahrs.pitch);
|
||||
pitch_limit_min_cd = aparm.pitch_limit_min_cd * fabsf(cosf(ahrs.roll));
|
||||
|
||||
// updated the summed gyro used for ground steering and
|
||||
// auto-takeoff. Dot product of DCM.c with gyro vector gives earth
|
||||
// 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 50Hz speed/height controller
|
||||
*/
|
||||
void Plane::update_speed_height(void)
|
||||
{
|
||||
if (auto_throttle_mode) {
|
||||
// Call TECS 50Hz update. Note that we call this regardless of
|
||||
// throttle suppressed, as this needs to be running for
|
||||
// takeoff detection
|
||||
SpdHgt_Controller->update_50hz(tecs_hgt_afe());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update camera mount
|
||||
*/
|
||||
void Plane::update_mount(void)
|
||||
{
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update();
|
||||
#endif
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
camera.trigger_pic_cleanup();
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
read and update compass
|
||||
*/
|
||||
void Plane::update_compass(void)
|
||||
{
|
||||
if (g.compass_enabled && compass.read()) {
|
||||
ahrs.set_compass(&compass);
|
||||
compass.learn_offsets();
|
||||
if (should_log(MASK_LOG_COMPASS)) {
|
||||
DataFlash.Log_Write_Compass(compass);
|
||||
}
|
||||
} else {
|
||||
ahrs.set_compass(NULL);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
if the compass is enabled then try to accumulate a reading
|
||||
*/
|
||||
void Plane::compass_accumulate(void)
|
||||
{
|
||||
if (g.compass_enabled) {
|
||||
compass.accumulate();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
try to accumulate a baro reading
|
||||
*/
|
||||
void Plane::barometer_accumulate(void)
|
||||
{
|
||||
barometer.accumulate();
|
||||
}
|
||||
|
||||
/*
|
||||
do 10Hz logging
|
||||
*/
|
||||
void Plane::update_logging1(void)
|
||||
{
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
}
|
||||
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_IMU))
|
||||
Log_Write_IMU();
|
||||
}
|
||||
|
||||
/*
|
||||
do 10Hz logging - part2
|
||||
*/
|
||||
void Plane::update_logging2(void)
|
||||
{
|
||||
if (should_log(MASK_LOG_CTUN))
|
||||
Log_Write_Control_Tuning();
|
||||
|
||||
if (should_log(MASK_LOG_NTUN))
|
||||
Log_Write_Nav_Tuning();
|
||||
|
||||
if (should_log(MASK_LOG_RC))
|
||||
Log_Write_RC();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
check for OBC failsafe check
|
||||
*/
|
||||
void Plane::obc_fs_check(void)
|
||||
{
|
||||
#if OBC_FAILSAFE == ENABLED
|
||||
// perform OBC failsafe checks
|
||||
obc.check(OBC_MODE(control_mode), failsafe.last_heartbeat_ms, geofence_breached(), failsafe.last_valid_rc_ms);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
update aux servo mappings
|
||||
*/
|
||||
void Plane::update_aux(void)
|
||||
{
|
||||
if (!px4io_override_enabled) {
|
||||
RC_Channel_aux::enable_aux_servos();
|
||||
}
|
||||
}
|
||||
|
||||
void Plane::one_second_loop()
|
||||
{
|
||||
if (should_log(MASK_LOG_CURRENT))
|
||||
Log_Write_Current();
|
||||
|
||||
// send a heartbeat
|
||||
gcs_send_message(MSG_HEARTBEAT);
|
||||
|
||||
// make it possible to change control channel ordering at runtime
|
||||
set_control_channels();
|
||||
|
||||
// make it possible to change orientation at runtime
|
||||
ahrs.set_orientation();
|
||||
|
||||
// sync MAVLink system ID
|
||||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
update_aux();
|
||||
|
||||
// determine if we are flying or not
|
||||
determine_is_flying();
|
||||
|
||||
// update notify flags
|
||||
AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
|
||||
AP_Notify::flags.pre_arm_gps_check = true;
|
||||
AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
|
||||
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
if (should_log(MASK_LOG_GPS)) {
|
||||
terrain.log_terrain_data(DataFlash);
|
||||
}
|
||||
#endif
|
||||
// piggyback the status log entry on the MODE log entry flag
|
||||
if (should_log(MASK_LOG_MODE)) {
|
||||
Log_Write_Status();
|
||||
}
|
||||
|
||||
ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW));
|
||||
}
|
||||
|
||||
void Plane::log_perf_info()
|
||||
{
|
||||
if (scheduler.debug() != 0) {
|
||||
gcs_send_text_fmt(PSTR("G_Dt_max=%lu G_Dt_min=%lu\n"),
|
||||
(unsigned long)G_Dt_max,
|
||||
(unsigned long)G_Dt_min);
|
||||
}
|
||||
if (should_log(MASK_LOG_PM))
|
||||
Log_Write_Performance();
|
||||
G_Dt_max = 0;
|
||||
G_Dt_min = 0;
|
||||
resetPerfData();
|
||||
}
|
||||
|
||||
void Plane::compass_save()
|
||||
{
|
||||
if (g.compass_enabled) {
|
||||
compass.save_offsets();
|
||||
}
|
||||
}
|
||||
|
||||
void Plane::terrain_update(void)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
terrain.update();
|
||||
|
||||
// tell the rangefinder our height, so it can go into power saving
|
||||
// mode if available
|
||||
float height;
|
||||
if (terrain.height_above_terrain(height, true)) {
|
||||
rangefinder.set_estimated_terrain_height(height);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
once a second update the airspeed calibration ratio
|
||||
*/
|
||||
void Plane::airspeed_ratio_update(void)
|
||||
{
|
||||
if (!airspeed.enabled() ||
|
||||
gps.status() < AP_GPS::GPS_OK_FIX_3D ||
|
||||
gps.ground_speed() < 4) {
|
||||
// don't calibrate when not moving
|
||||
return;
|
||||
}
|
||||
if (airspeed.get_airspeed() < aparm.airspeed_min &&
|
||||
gps.ground_speed() < (uint32_t)aparm.airspeed_min) {
|
||||
// don't calibrate when flying below the minimum airspeed. We
|
||||
// check both airspeed and ground speed to catch cases where
|
||||
// the airspeed ratio is way too low, which could lead to it
|
||||
// never coming up again
|
||||
return;
|
||||
}
|
||||
if (abs(ahrs.roll_sensor) > roll_limit_cd ||
|
||||
ahrs.pitch_sensor > aparm.pitch_limit_max_cd ||
|
||||
ahrs.pitch_sensor < pitch_limit_min_cd) {
|
||||
// don't calibrate when going beyond normal flight envelope
|
||||
return;
|
||||
}
|
||||
const Vector3f &vg = gps.velocity();
|
||||
airspeed.update_calibration(vg);
|
||||
gcs_send_airspeed_calibration(vg);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
read the GPS and update position
|
||||
*/
|
||||
void Plane::update_GPS_50Hz(void)
|
||||
{
|
||||
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
|
||||
gps.update();
|
||||
|
||||
for (uint8_t i=0; i<gps.num_sensors(); i++) {
|
||||
if (gps.last_message_time_ms(i) != last_gps_reading[i]) {
|
||||
last_gps_reading[i] = gps.last_message_time_ms(i);
|
||||
if (should_log(MASK_LOG_GPS)) {
|
||||
Log_Write_GPS(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
read update GPS position - 10Hz update
|
||||
*/
|
||||
void Plane::update_GPS_10Hz(void)
|
||||
{
|
||||
// get position from AHRS
|
||||
have_position = ahrs.get_position(current_loc);
|
||||
|
||||
static uint32_t last_gps_msg_ms;
|
||||
if (gps.last_message_time_ms() != last_gps_msg_ms && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
last_gps_msg_ms = gps.last_message_time_ms();
|
||||
|
||||
if (ground_start_count > 1) {
|
||||
ground_start_count--;
|
||||
} else if (ground_start_count == 1) {
|
||||
// We countdown N number of good GPS fixes
|
||||
// so that the altitude is more accurate
|
||||
// -------------------------------------
|
||||
if (current_loc.lat == 0) {
|
||||
ground_start_count = 5;
|
||||
|
||||
} else {
|
||||
init_home();
|
||||
|
||||
// set system clock for log timestamps
|
||||
hal.util->set_system_clock(gps.time_epoch_usec());
|
||||
|
||||
if (g.compass_enabled) {
|
||||
// Set compass declination automatically
|
||||
const Location &loc = gps.location();
|
||||
compass.set_initial_location(loc.lat, loc.lng);
|
||||
}
|
||||
ground_start_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// see if we've breached the geo-fence
|
||||
geofence_check(false);
|
||||
|
||||
#if CAMERA == ENABLED
|
||||
if (camera.update_location(current_loc) == true) {
|
||||
do_take_picture();
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!hal.util->get_soft_armed()) {
|
||||
update_home();
|
||||
}
|
||||
|
||||
// update wind estimate
|
||||
ahrs.estimate_wind();
|
||||
}
|
||||
|
||||
calc_gndspeed_undershoot();
|
||||
}
|
||||
|
||||
/*
|
||||
main handling for AUTO mode
|
||||
*/
|
||||
void Plane::handle_auto_mode(void)
|
||||
{
|
||||
uint8_t nav_cmd_id;
|
||||
|
||||
// we should be either running a mission or RTLing home
|
||||
if (mission.state() == AP_Mission::MISSION_RUNNING) {
|
||||
nav_cmd_id = mission.get_current_nav_cmd().id;
|
||||
}else{
|
||||
nav_cmd_id = auto_rtl_command.id;
|
||||
}
|
||||
|
||||
switch(nav_cmd_id) {
|
||||
case MAV_CMD_NAV_TAKEOFF:
|
||||
takeoff_calc_roll();
|
||||
takeoff_calc_pitch();
|
||||
calc_throttle();
|
||||
|
||||
break;
|
||||
|
||||
case MAV_CMD_NAV_LAND:
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
|
||||
if (auto_state.land_complete) {
|
||||
// during final approach constrain roll to the range
|
||||
// allowed for level flight
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
|
||||
}
|
||||
calc_throttle();
|
||||
|
||||
if (auto_state.land_complete) {
|
||||
// we are in the final stage of a landing - force
|
||||
// zero throttle
|
||||
channel_throttle->servo_out = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
// we are doing normal AUTO flight, the special cases
|
||||
// are for takeoff and landing
|
||||
steer_state.hold_course_cd = -1;
|
||||
auto_state.land_complete = false;
|
||||
auto_state.land_sink_rate = 0;
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
calc_throttle();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
main flight mode dependent update code
|
||||
*/
|
||||
void Plane::update_flight_mode(void)
|
||||
{
|
||||
enum FlightMode effective_mode = control_mode;
|
||||
if (control_mode == AUTO && g.auto_fbw_steer) {
|
||||
effective_mode = FLY_BY_WIRE_A;
|
||||
}
|
||||
|
||||
if (effective_mode != AUTO) {
|
||||
// hold_course is only used in takeoff and landing
|
||||
steer_state.hold_course_cd = -1;
|
||||
}
|
||||
|
||||
switch (effective_mode)
|
||||
{
|
||||
case AUTO:
|
||||
handle_auto_mode();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
case LOITER:
|
||||
case GUIDED:
|
||||
calc_nav_roll();
|
||||
calc_nav_pitch();
|
||||
calc_throttle();
|
||||
break;
|
||||
|
||||
case TRAINING: {
|
||||
training_manual_roll = false;
|
||||
training_manual_pitch = false;
|
||||
|
||||
// if the roll is past the set roll limit, then
|
||||
// we set target roll to the limit
|
||||
if (ahrs.roll_sensor >= roll_limit_cd) {
|
||||
nav_roll_cd = roll_limit_cd;
|
||||
} else if (ahrs.roll_sensor <= -roll_limit_cd) {
|
||||
nav_roll_cd = -roll_limit_cd;
|
||||
} else {
|
||||
training_manual_roll = true;
|
||||
nav_roll_cd = 0;
|
||||
}
|
||||
|
||||
// if the pitch is past the set pitch limits, then
|
||||
// we set target pitch to the limit
|
||||
if (ahrs.pitch_sensor >= aparm.pitch_limit_max_cd) {
|
||||
nav_pitch_cd = aparm.pitch_limit_max_cd;
|
||||
} else if (ahrs.pitch_sensor <= pitch_limit_min_cd) {
|
||||
nav_pitch_cd = pitch_limit_min_cd;
|
||||
} else {
|
||||
training_manual_pitch = true;
|
||||
nav_pitch_cd = 0;
|
||||
}
|
||||
if (fly_inverted()) {
|
||||
nav_pitch_cd = -nav_pitch_cd;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case ACRO: {
|
||||
// handle locked/unlocked control
|
||||
if (acro_state.locked_roll) {
|
||||
nav_roll_cd = acro_state.locked_roll_err;
|
||||
} else {
|
||||
nav_roll_cd = ahrs.roll_sensor;
|
||||
}
|
||||
if (acro_state.locked_pitch) {
|
||||
nav_pitch_cd = acro_state.locked_pitch_cd;
|
||||
} else {
|
||||
nav_pitch_cd = ahrs.pitch_sensor;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AUTOTUNE:
|
||||
case FLY_BY_WIRE_A: {
|
||||
// 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);
|
||||
update_load_factor();
|
||||
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);
|
||||
}
|
||||
adjust_nav_pitch_throttle();
|
||||
nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get());
|
||||
if (fly_inverted()) {
|
||||
nav_pitch_cd = -nav_pitch_cd;
|
||||
}
|
||||
if (failsafe.ch3_failsafe && g.short_fs_action == 2) {
|
||||
// FBWA failsafe glide
|
||||
nav_roll_cd = 0;
|
||||
nav_pitch_cd = 0;
|
||||
channel_throttle->servo_out = 0;
|
||||
}
|
||||
if (g.fbwa_tdrag_chan > 0) {
|
||||
// check for the user enabling FBWA taildrag takeoff mode
|
||||
bool tdrag_mode = (hal.rcin->read(g.fbwa_tdrag_chan-1) > 1700);
|
||||
if (tdrag_mode && !auto_state.fbwa_tdrag_takeoff_mode) {
|
||||
if (auto_state.highest_airspeed < g.takeoff_tdrag_speed1) {
|
||||
auto_state.fbwa_tdrag_takeoff_mode = true;
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("FBWA tdrag mode\n"));
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case FLY_BY_WIRE_B:
|
||||
// Thanks to Yury MonZon for the altitude limit code!
|
||||
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
|
||||
update_load_factor();
|
||||
update_fbwb_speed_height();
|
||||
break;
|
||||
|
||||
case CRUISE:
|
||||
/*
|
||||
in CRUISE mode we use the navigation code to control
|
||||
roll when heading is locked. Heading becomes unlocked on
|
||||
any aileron or rudder input
|
||||
*/
|
||||
if ((channel_roll->control_in != 0 ||
|
||||
rudder_input != 0)) {
|
||||
cruise_state.locked_heading = false;
|
||||
cruise_state.lock_timer_ms = 0;
|
||||
}
|
||||
|
||||
if (!cruise_state.locked_heading) {
|
||||
nav_roll_cd = channel_roll->norm_input() * roll_limit_cd;
|
||||
nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit_cd, roll_limit_cd);
|
||||
update_load_factor();
|
||||
} else {
|
||||
calc_nav_roll();
|
||||
}
|
||||
update_fbwb_speed_height();
|
||||
break;
|
||||
|
||||
case STABILIZE:
|
||||
nav_roll_cd = 0;
|
||||
nav_pitch_cd = 0;
|
||||
// throttle is passthrough
|
||||
break;
|
||||
|
||||
case CIRCLE:
|
||||
// we have no GPS installed and have lost radio contact
|
||||
// or we just want to fly around in a gentle circle w/o GPS,
|
||||
// holding altitude at the altitude we set when we
|
||||
// switched into the mode
|
||||
nav_roll_cd = roll_limit_cd / 3;
|
||||
update_load_factor();
|
||||
calc_nav_pitch();
|
||||
calc_throttle();
|
||||
break;
|
||||
|
||||
case MANUAL:
|
||||
// servo_out is for Sim control only
|
||||
// ---------------------------------
|
||||
channel_roll->servo_out = channel_roll->pwm_to_angle();
|
||||
channel_pitch->servo_out = channel_pitch->pwm_to_angle();
|
||||
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 INITIALISING:
|
||||
// handled elsewhere
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void Plane::update_navigation()
|
||||
{
|
||||
// wp_distance is in ACTUAL meters, not the *100 meters we get from the GPS
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
// distance and bearing calcs only
|
||||
switch(control_mode) {
|
||||
case AUTO:
|
||||
update_commands();
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
if (g.rtl_autoland == 1 &&
|
||||
!auto_state.checked_for_autoland &&
|
||||
nav_controller->reached_loiter_target() &&
|
||||
labs(altitude_error_cm) < 1000) {
|
||||
// we've reached the RTL point, see if we have a landing sequence
|
||||
jump_to_landing_sequence();
|
||||
|
||||
// prevent running the expensive jump_to_landing_sequence
|
||||
// on every loop
|
||||
auto_state.checked_for_autoland = true;
|
||||
}
|
||||
else if (g.rtl_autoland == 2 &&
|
||||
!auto_state.checked_for_autoland) {
|
||||
// Go directly to the landing sequence
|
||||
jump_to_landing_sequence();
|
||||
|
||||
// prevent running the expensive jump_to_landing_sequence
|
||||
// on every loop
|
||||
auto_state.checked_for_autoland = true;
|
||||
}
|
||||
// fall through to LOITER
|
||||
|
||||
case LOITER:
|
||||
case GUIDED:
|
||||
// allow loiter direction to be changed in flight
|
||||
if (g.loiter_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
} else {
|
||||
loiter.direction = 1;
|
||||
}
|
||||
update_loiter();
|
||||
break;
|
||||
|
||||
case CRUISE:
|
||||
update_cruise();
|
||||
break;
|
||||
|
||||
case MANUAL:
|
||||
case STABILIZE:
|
||||
case TRAINING:
|
||||
case INITIALISING:
|
||||
case ACRO:
|
||||
case FLY_BY_WIRE_A:
|
||||
case AUTOTUNE:
|
||||
case FLY_BY_WIRE_B:
|
||||
case CIRCLE:
|
||||
// nothing to do
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
set the flight stage
|
||||
*/
|
||||
void Plane::set_flight_stage(AP_SpdHgtControl::FlightStage fs)
|
||||
{
|
||||
//if just now entering land flight stage
|
||||
if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
|
||||
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
||||
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
if (g.fence_autoenable == 1) {
|
||||
if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence failed (autodisable)"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence disabled (autodisable)"));
|
||||
}
|
||||
} else if (g.fence_autoenable == 2) {
|
||||
if (! geofence_set_floor_enabled(false)) {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Disable fence floor failed (autodisable)"));
|
||||
} else {
|
||||
gcs_send_text_P(SEVERITY_HIGH, PSTR("Fence floor disabled (auto disable)"));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
flight_stage = fs;
|
||||
}
|
||||
|
||||
void Plane::update_alt()
|
||||
{
|
||||
barometer.update();
|
||||
if (should_log(MASK_LOG_IMU)) {
|
||||
Log_Write_Baro();
|
||||
}
|
||||
|
||||
geofence_check(true);
|
||||
|
||||
update_flight_stage();
|
||||
}
|
||||
|
||||
/*
|
||||
recalculate the flight_stage
|
||||
*/
|
||||
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) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
|
||||
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND &&
|
||||
auto_state.land_complete == true) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
|
||||
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH);
|
||||
} else {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
|
||||
}
|
||||
} else {
|
||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
|
||||
}
|
||||
|
||||
SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),
|
||||
target_airspeed_cm,
|
||||
flight_stage,
|
||||
auto_state.takeoff_pitch_cd,
|
||||
throttle_nudge,
|
||||
tecs_hgt_afe(),
|
||||
aerodynamic_load_factor);
|
||||
if (should_log(MASK_LOG_TECS)) {
|
||||
Log_Write_TECS_Tuning();
|
||||
}
|
||||
}
|
||||
|
||||
// tell AHRS the airspeed to true airspeed ratio
|
||||
airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
Do we think we are flying?
|
||||
Probabilistic method where a bool is low-passed and considered a probability.
|
||||
*/
|
||||
void Plane::determine_is_flying(void)
|
||||
{
|
||||
float aspeed;
|
||||
bool isFlyingBool;
|
||||
|
||||
bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5);
|
||||
|
||||
// If we don't have a GPS lock then don't use GPS for this test
|
||||
bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
|
||||
gps.ground_speed() >= 5);
|
||||
|
||||
|
||||
if (hal.util->get_soft_armed()) {
|
||||
// when armed, we need overwhelming evidence that we ARE NOT flying
|
||||
isFlyingBool = airspeedMovement || gpsMovement;
|
||||
|
||||
/*
|
||||
make is_flying() more accurate for landing approach
|
||||
*/
|
||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
|
||||
fabsf(auto_state.land_sink_rate) > 0.2f) {
|
||||
isFlyingBool = true;
|
||||
}
|
||||
} else {
|
||||
// when disarmed, we need overwhelming evidence that we ARE flying
|
||||
isFlyingBool = airspeedMovement && gpsMovement;
|
||||
}
|
||||
|
||||
// low-pass the result.
|
||||
isFlyingProbability = (0.6f * isFlyingProbability) + (0.4f * (float)isFlyingBool);
|
||||
|
||||
/*
|
||||
update last_flying_ms so we always know how long we have not
|
||||
been flying for. This helps for crash detection and auto-disarm
|
||||
*/
|
||||
if (is_flying()) {
|
||||
auto_state.last_flying_ms = hal.scheduler->millis();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
return true if we think we are flying. This is a probabilistic
|
||||
estimate, and needs to be used very carefully. Each use case needs
|
||||
to be thought about individually.
|
||||
*/
|
||||
bool Plane::is_flying(void)
|
||||
{
|
||||
if (hal.util->get_soft_armed()) {
|
||||
// when armed, assume we're flying unless we probably aren't
|
||||
return (isFlyingProbability >= 0.1f);
|
||||
}
|
||||
|
||||
// when disarmed, assume we're not flying unless we probably are
|
||||
return (isFlyingProbability >= 0.9f);
|
||||
}
|
||||
|
||||
|
||||
#if OPTFLOW == ENABLED
|
||||
// called at 50hz
|
||||
void Plane::update_optical_flow(void)
|
||||
{
|
||||
static uint32_t last_of_update = 0;
|
||||
|
||||
// exit immediately if not enabled
|
||||
if (!optflow.enabled()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// read from sensor
|
||||
optflow.update();
|
||||
|
||||
// write to log and send to EKF if new data has arrived
|
||||
if (optflow.last_update() != last_of_update) {
|
||||
last_of_update = optflow.last_update();
|
||||
uint8_t flowQuality = optflow.quality();
|
||||
Vector2f flowRate = optflow.flowRate();
|
||||
Vector2f bodyRate = optflow.bodyRate();
|
||||
ahrs.writeOptFlowMeas(flowQuality, flowRate, bodyRate, last_of_update);
|
||||
Log_Write_Optflow();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
compatibility with old pde style build
|
||||
*/
|
||||
void setup(void);
|
||||
void loop(void);
|
||||
|
||||
void setup(void)
|
||||
{
|
||||
plane.setup();
|
||||
}
|
||||
void loop(void)
|
||||
{
|
||||
plane.loop();
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
|
|
@ -1,16 +1,13 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
//****************************************************************
|
||||
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
|
||||
//****************************************************************
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
get a speed scaling number for control surfaces. This is applied to
|
||||
PIDs to change the scaling of the PID with speed. At high speed we
|
||||
move the surfaces less, and at low speeds we move them more.
|
||||
*/
|
||||
static float get_speed_scaler(void)
|
||||
float Plane::get_speed_scaler(void)
|
||||
{
|
||||
float aspeed, speed_scaler;
|
||||
if (ahrs.airspeed_estimate(&aspeed)) {
|
||||
|
|
@ -39,7 +36,7 @@ static float get_speed_scaler(void)
|
|||
/*
|
||||
return true if the current settings and mode should allow for stick mixing
|
||||
*/
|
||||
static bool stick_mixing_enabled(void)
|
||||
bool Plane::stick_mixing_enabled(void)
|
||||
{
|
||||
if (auto_throttle_mode) {
|
||||
// we're in an auto mode. Check the stick mixing flag
|
||||
|
|
@ -69,7 +66,7 @@ static bool stick_mixing_enabled(void)
|
|||
previously set nav_roll calculates roll servo_out to try to
|
||||
stabilize the plane at the given roll
|
||||
*/
|
||||
static void stabilize_roll(float speed_scaler)
|
||||
void Plane::stabilize_roll(float speed_scaler)
|
||||
{
|
||||
if (fly_inverted()) {
|
||||
// we want to fly upside down. We need to cope with wrap of
|
||||
|
|
@ -95,7 +92,7 @@ static void stabilize_roll(float speed_scaler)
|
|||
previously set nav_pitch and calculates servo_out values to try to
|
||||
stabilize the plane at the given attitude.
|
||||
*/
|
||||
static void stabilize_pitch(float speed_scaler)
|
||||
void Plane::stabilize_pitch(float speed_scaler)
|
||||
{
|
||||
int8_t force_elevator = takeoff_tail_hold();
|
||||
if (force_elevator != 0) {
|
||||
|
|
@ -120,7 +117,7 @@ static void stabilize_pitch(float speed_scaler)
|
|||
controller as it increases the influence of the users stick input,
|
||||
allowing the user full deflection if needed
|
||||
*/
|
||||
static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
|
||||
void Plane::stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
|
||||
{
|
||||
float ch_inf;
|
||||
|
||||
|
|
@ -135,7 +132,7 @@ static void stick_mix_channel(RC_Channel *channel, int16_t &servo_out)
|
|||
/*
|
||||
this gives the user control of the aircraft in stabilization modes
|
||||
*/
|
||||
static void stabilize_stick_mixing_direct()
|
||||
void Plane::stabilize_stick_mixing_direct()
|
||||
{
|
||||
if (!stick_mixing_enabled() ||
|
||||
control_mode == ACRO ||
|
||||
|
|
@ -154,7 +151,7 @@ static void stabilize_stick_mixing_direct()
|
|||
this gives the user control of the aircraft in stabilization modes
|
||||
using FBW style controls
|
||||
*/
|
||||
static void stabilize_stick_mixing_fbw()
|
||||
void Plane::stabilize_stick_mixing_fbw()
|
||||
{
|
||||
if (!stick_mixing_enabled() ||
|
||||
control_mode == ACRO ||
|
||||
|
|
@ -204,7 +201,7 @@ static void stabilize_stick_mixing_fbw()
|
|||
- rate controlled with ground steering
|
||||
- yaw control for coordinated flight
|
||||
*/
|
||||
static void stabilize_yaw(float speed_scaler)
|
||||
void Plane::stabilize_yaw(float speed_scaler)
|
||||
{
|
||||
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
|
||||
// in land final setup for ground steering
|
||||
|
|
@ -245,7 +242,7 @@ static void stabilize_yaw(float speed_scaler)
|
|||
/*
|
||||
a special stabilization function for training mode
|
||||
*/
|
||||
static void stabilize_training(float speed_scaler)
|
||||
void Plane::stabilize_training(float speed_scaler)
|
||||
{
|
||||
if (training_manual_roll) {
|
||||
channel_roll->servo_out = channel_roll->control_in;
|
||||
|
|
@ -278,7 +275,7 @@ static void stabilize_training(float speed_scaler)
|
|||
this is the ACRO mode stabilization function. It does rate
|
||||
stabilization on roll and pitch axes
|
||||
*/
|
||||
static void stabilize_acro(float speed_scaler)
|
||||
void Plane::stabilize_acro(float speed_scaler)
|
||||
{
|
||||
float roll_rate = (channel_roll->control_in/4500.0f) * g.acro_roll_rate;
|
||||
float pitch_rate = (channel_pitch->control_in/4500.0f) * g.acro_pitch_rate;
|
||||
|
|
@ -345,7 +342,7 @@ static void stabilize_acro(float speed_scaler)
|
|||
/*
|
||||
main stabilization function for all 3 axes
|
||||
*/
|
||||
static void stabilize()
|
||||
void Plane::stabilize()
|
||||
{
|
||||
if (control_mode == MANUAL) {
|
||||
// nothing to do
|
||||
|
|
@ -391,7 +388,7 @@ static void stabilize()
|
|||
}
|
||||
|
||||
|
||||
static void calc_throttle()
|
||||
void Plane::calc_throttle()
|
||||
{
|
||||
if (aparm.throttle_cruise <= 1) {
|
||||
// user has asked for zero throttle - this may be done by a
|
||||
|
|
@ -411,7 +408,7 @@ static void calc_throttle()
|
|||
/*
|
||||
calculate yaw control for coordinated flight
|
||||
*/
|
||||
static void calc_nav_yaw_coordinated(float speed_scaler)
|
||||
void Plane::calc_nav_yaw_coordinated(float speed_scaler)
|
||||
{
|
||||
bool disable_integrator = false;
|
||||
if (control_mode == STABILIZE && rudder_input != 0) {
|
||||
|
|
@ -428,7 +425,7 @@ static void calc_nav_yaw_coordinated(float speed_scaler)
|
|||
/*
|
||||
calculate yaw control for ground steering with specific course
|
||||
*/
|
||||
static void calc_nav_yaw_course(void)
|
||||
void Plane::calc_nav_yaw_course(void)
|
||||
{
|
||||
// holding a specific navigation course on the ground. Used in
|
||||
// auto-takeoff and landing
|
||||
|
|
@ -443,7 +440,7 @@ static void calc_nav_yaw_course(void)
|
|||
/*
|
||||
calculate yaw control for ground steering
|
||||
*/
|
||||
static void calc_nav_yaw_ground(void)
|
||||
void Plane::calc_nav_yaw_ground(void)
|
||||
{
|
||||
if (gps.ground_speed() < 1 &&
|
||||
channel_throttle->control_in == 0 &&
|
||||
|
|
@ -484,7 +481,7 @@ static void calc_nav_yaw_ground(void)
|
|||
/*
|
||||
calculate a new nav_pitch_cd from the speed height controller
|
||||
*/
|
||||
static void calc_nav_pitch()
|
||||
void Plane::calc_nav_pitch()
|
||||
{
|
||||
// Calculate the Pitch of the plane
|
||||
// --------------------------------
|
||||
|
|
@ -496,7 +493,7 @@ static void calc_nav_pitch()
|
|||
/*
|
||||
calculate a new nav_roll_cd from the navigation controller
|
||||
*/
|
||||
static void calc_nav_roll()
|
||||
void Plane::calc_nav_roll()
|
||||
{
|
||||
nav_roll_cd = nav_controller->nav_roll_cd();
|
||||
update_load_factor();
|
||||
|
|
@ -507,7 +504,7 @@ static void calc_nav_roll()
|
|||
/*****************************************
|
||||
* Throttle slew limit
|
||||
*****************************************/
|
||||
static void throttle_slew_limit(int16_t last_throttle)
|
||||
void Plane::throttle_slew_limit(int16_t last_throttle)
|
||||
{
|
||||
uint8_t slewrate = aparm.throttle_slewrate;
|
||||
if (control_mode==AUTO && auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) {
|
||||
|
|
@ -528,7 +525,7 @@ static void throttle_slew_limit(int16_t last_throttle)
|
|||
/*****************************************
|
||||
Flap slew limit
|
||||
*****************************************/
|
||||
static void flap_slew_limit(int8_t &last_value, int8_t &new_value)
|
||||
void Plane::flap_slew_limit(int8_t &last_value, int8_t &new_value)
|
||||
{
|
||||
uint8_t slewrate = g.flap_slewrate;
|
||||
// if slew limit rate is set to zero then do not slew limit
|
||||
|
|
@ -556,7 +553,7 @@ static void flap_slew_limit(int8_t &last_value, int8_t &new_value)
|
|||
* OR
|
||||
* 5 - Home location is not set
|
||||
*/
|
||||
static bool suppress_throttle(void)
|
||||
bool Plane::suppress_throttle(void)
|
||||
{
|
||||
if (!throttle_suppressed) {
|
||||
// we've previously met a condition for unsupressing the throttle
|
||||
|
|
@ -614,7 +611,7 @@ static bool suppress_throttle(void)
|
|||
/*
|
||||
implement a software VTail or elevon mixer. There are 4 different mixing modes
|
||||
*/
|
||||
static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out)
|
||||
void Plane::channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out)
|
||||
{
|
||||
int16_t c1, c2;
|
||||
int16_t v1, v2;
|
||||
|
|
@ -659,7 +656,7 @@ static void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_
|
|||
/*
|
||||
setup flaperon output channels
|
||||
*/
|
||||
static void flaperon_update(int8_t flap_percent)
|
||||
void Plane::flaperon_update(int8_t flap_percent)
|
||||
{
|
||||
if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) ||
|
||||
!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) {
|
||||
|
|
@ -688,7 +685,7 @@ static void flaperon_update(int8_t flap_percent)
|
|||
/*****************************************
|
||||
* Set the flight control servos based on the current calculated values
|
||||
*****************************************/
|
||||
static void set_servos(void)
|
||||
void Plane::set_servos(void)
|
||||
{
|
||||
int16_t last_throttle = channel_throttle->radio_out;
|
||||
|
||||
|
|
@ -980,20 +977,18 @@ static void set_servos(void)
|
|||
RC_Channel_aux::output_ch_all();
|
||||
}
|
||||
|
||||
static bool demoing_servos;
|
||||
|
||||
static void demo_servos(uint8_t i)
|
||||
void Plane::demo_servos(uint8_t i)
|
||||
{
|
||||
while(i > 0) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
|
||||
demoing_servos = true;
|
||||
servo_write(1, 1400);
|
||||
mavlink_delay(400);
|
||||
hal.scheduler->delay(400);
|
||||
servo_write(1, 1600);
|
||||
mavlink_delay(200);
|
||||
hal.scheduler->delay(200);
|
||||
servo_write(1, 1500);
|
||||
demoing_servos = false;
|
||||
mavlink_delay(400);
|
||||
hal.scheduler->delay(400);
|
||||
i--;
|
||||
}
|
||||
}
|
||||
|
|
@ -1004,7 +999,7 @@ static void demo_servos(uint8_t i)
|
|||
automatically pitch down a little when at low throttle. It makes
|
||||
FBWA landings without stalling much easier.
|
||||
*/
|
||||
static void adjust_nav_pitch_throttle(void)
|
||||
void Plane::adjust_nav_pitch_throttle(void)
|
||||
{
|
||||
uint8_t throttle = throttle_percentage();
|
||||
if (throttle < aparm.throttle_cruise) {
|
||||
|
|
@ -1019,7 +1014,7 @@ static void adjust_nav_pitch_throttle(void)
|
|||
ensure that the load factor does not take us below the sustainable
|
||||
airspeed
|
||||
*/
|
||||
static void update_load_factor(void)
|
||||
void Plane::update_load_factor(void)
|
||||
{
|
||||
float demanded_roll = fabsf(nav_roll_cd*0.01f);
|
||||
if (demanded_roll > 85) {
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,33 +1,28 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
// Code to Write and Read packets from DataFlash.log memory
|
||||
// Code to interact with the user to dump or erase logs
|
||||
|
||||
// These are function definitions so the Menu can be constructed before the functions
|
||||
// are defined below. Order matters to the compiler.
|
||||
static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
// Creates a constant array of structs representing menu options
|
||||
// and stores them in Flash memory, not RAM.
|
||||
// User enters the string in the console to call the functions on the right.
|
||||
// See class Menu in AP_Coommon for implementation details
|
||||
static const struct Menu::command log_menu_commands[] PROGMEM = {
|
||||
{"dump", dump_log},
|
||||
{"erase", erase_logs},
|
||||
{"enable", select_logs},
|
||||
{"disable", select_logs}
|
||||
{"dump", MENU_FUNC(dump_log)},
|
||||
{"erase", MENU_FUNC(erase_logs)},
|
||||
{"enable", MENU_FUNC(select_logs)},
|
||||
{"disable", MENU_FUNC(select_logs)}
|
||||
};
|
||||
|
||||
// A Macro to create the Menu
|
||||
MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
|
||||
MENU2(log_menu, "Log", log_menu_commands, MENU_FUNC(print_log_menu));
|
||||
|
||||
static bool
|
||||
print_log_menu(void)
|
||||
bool Plane::print_log_menu(void)
|
||||
{
|
||||
cliSerial->println_P(PSTR("logs enabled: "));
|
||||
|
||||
|
|
@ -63,39 +58,37 @@ print_log_menu(void)
|
|||
return(true);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int16_t dump_log;
|
||||
int16_t dump_log_num;
|
||||
uint16_t dump_log_start;
|
||||
uint16_t dump_log_end;
|
||||
uint16_t last_log_num;
|
||||
|
||||
// check that the requested log number can be read
|
||||
dump_log = argv[1].i;
|
||||
dump_log_num = argv[1].i;
|
||||
last_log_num = DataFlash.find_last_log();
|
||||
|
||||
if (dump_log == -2) {
|
||||
if (dump_log_num == -2) {
|
||||
DataFlash.DumpPageInfo(cliSerial);
|
||||
return(-1);
|
||||
} else if (dump_log <= 0) {
|
||||
} else if (dump_log_num <= 0) {
|
||||
cliSerial->printf_P(PSTR("dumping all\n"));
|
||||
Log_Read(0, 1, 0);
|
||||
return(-1);
|
||||
} else if ((argc != 2)
|
||||
|| ((uint16_t)dump_log > last_log_num))
|
||||
|| ((uint16_t)dump_log_num > last_log_num))
|
||||
{
|
||||
cliSerial->printf_P(PSTR("bad log number\n"));
|
||||
return(-1);
|
||||
}
|
||||
|
||||
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
|
||||
Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
|
||||
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
|
||||
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int8_t
|
||||
erase_logs(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
in_mavlink_delay = true;
|
||||
do_erase_logs();
|
||||
|
|
@ -103,8 +96,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int8_t
|
||||
select_logs(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
uint32_t bits;
|
||||
|
||||
|
|
@ -151,8 +143,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
|||
return(0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
process_logs(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
log_menu.run();
|
||||
return 0;
|
||||
|
|
@ -160,7 +151,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
|
|||
|
||||
#endif // CLI_ENABLED == ENABLED
|
||||
|
||||
static void do_erase_logs(void)
|
||||
void Plane::do_erase_logs(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
|
||||
DataFlash.EraseAll();
|
||||
|
|
@ -169,7 +160,7 @@ static void do_erase_logs(void)
|
|||
|
||||
|
||||
// Write an attitude packet
|
||||
static void Log_Write_Attitude(void)
|
||||
void Plane::Log_Write_Attitude(void)
|
||||
{
|
||||
Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
|
||||
targets.x = nav_roll_cd;
|
||||
|
|
@ -206,11 +197,11 @@ struct PACKED log_Performance {
|
|||
};
|
||||
|
||||
// Write a performance monitoring packet. Total length : 19 bytes
|
||||
static void Log_Write_Performance()
|
||||
void Plane::Log_Write_Performance()
|
||||
{
|
||||
struct log_Performance pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
|
||||
loop_time : millis() - perf_mon_timer,
|
||||
loop_time : hal.scheduler->millis() - perf_mon_timer,
|
||||
main_loop_count : mainLoop_count,
|
||||
g_dt_max : G_Dt_max,
|
||||
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000),
|
||||
|
|
@ -223,7 +214,7 @@ static void Log_Write_Performance()
|
|||
}
|
||||
|
||||
// Write a mission command. Total length : 36 bytes
|
||||
static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
|
||||
void Plane::Log_Write_Cmd(const AP_Mission::Mission_Command &cmd)
|
||||
{
|
||||
mavlink_mission_item_t mav_cmd = {};
|
||||
AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
|
||||
|
|
@ -236,7 +227,7 @@ struct PACKED log_Startup {
|
|||
uint16_t command_total;
|
||||
};
|
||||
|
||||
static void Log_Write_Startup(uint8_t type)
|
||||
void Plane::Log_Write_Startup(uint8_t type)
|
||||
{
|
||||
struct log_Startup pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
|
||||
|
|
@ -249,7 +240,7 @@ static void Log_Write_Startup(uint8_t type)
|
|||
Log_Write_EntireMission();
|
||||
}
|
||||
|
||||
static void Log_Write_EntireMission()
|
||||
void Plane::Log_Write_EntireMission()
|
||||
{
|
||||
DataFlash.Log_Write_Message_P(PSTR("New mission"));
|
||||
|
||||
|
|
@ -274,7 +265,7 @@ struct PACKED log_Control_Tuning {
|
|||
};
|
||||
|
||||
// Write a control tuning packet. Total length : 22 bytes
|
||||
static void Log_Write_Control_Tuning()
|
||||
void Plane::Log_Write_Control_Tuning()
|
||||
{
|
||||
Vector3f accel = ins.get_accel();
|
||||
struct log_Control_Tuning pkt = {
|
||||
|
|
@ -292,7 +283,7 @@ static void Log_Write_Control_Tuning()
|
|||
}
|
||||
|
||||
// Write a TECS tuning packet
|
||||
static void Log_Write_TECS_Tuning(void)
|
||||
void Plane::Log_Write_TECS_Tuning(void)
|
||||
{
|
||||
SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG);
|
||||
}
|
||||
|
|
@ -311,7 +302,7 @@ struct PACKED log_Nav_Tuning {
|
|||
};
|
||||
|
||||
// Write a navigation tuning packe
|
||||
static void Log_Write_Nav_Tuning()
|
||||
void Plane::Log_Write_Nav_Tuning()
|
||||
{
|
||||
struct log_Nav_Tuning pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
|
||||
|
|
@ -337,7 +328,7 @@ struct PACKED log_Status {
|
|||
uint8_t safety;
|
||||
};
|
||||
|
||||
static void Log_Write_Status()
|
||||
void Plane::Log_Write_Status()
|
||||
{
|
||||
struct log_Status pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
|
||||
|
|
@ -364,7 +355,7 @@ struct PACKED log_Sonar {
|
|||
};
|
||||
|
||||
// Write a sonar packet
|
||||
static void Log_Write_Sonar()
|
||||
void Plane::Log_Write_Sonar()
|
||||
{
|
||||
struct log_Sonar pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
|
||||
|
|
@ -392,7 +383,7 @@ struct PACKED log_Optflow {
|
|||
|
||||
#if OPTFLOW == ENABLED
|
||||
// Write an optical flow packet
|
||||
static void Log_Write_Optflow()
|
||||
void Plane::Log_Write_Optflow()
|
||||
{
|
||||
// exit immediately if not enabled
|
||||
if (!optflow.enabled()) {
|
||||
|
|
@ -420,7 +411,7 @@ struct PACKED log_Arm_Disarm {
|
|||
uint16_t arm_checks;
|
||||
};
|
||||
|
||||
static void Log_Write_Current()
|
||||
void Plane::Log_Write_Current()
|
||||
{
|
||||
DataFlash.Log_Write_Current(battery, channel_throttle->control_in);
|
||||
|
||||
|
|
@ -428,7 +419,7 @@ static void Log_Write_Current()
|
|||
DataFlash.Log_Write_Power();
|
||||
}
|
||||
|
||||
static void Log_Arm_Disarm() {
|
||||
void Plane::Log_Arm_Disarm() {
|
||||
struct log_Arm_Disarm pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
|
||||
time_ms : hal.scheduler->millis(),
|
||||
|
|
@ -438,29 +429,29 @@ static void Log_Arm_Disarm() {
|
|||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
||||
static void Log_Write_GPS(uint8_t instance)
|
||||
void Plane::Log_Write_GPS(uint8_t instance)
|
||||
{
|
||||
DataFlash.Log_Write_GPS(gps, instance, current_loc.alt - ahrs.get_home().alt);
|
||||
}
|
||||
|
||||
static void Log_Write_IMU()
|
||||
void Plane::Log_Write_IMU()
|
||||
{
|
||||
DataFlash.Log_Write_IMU(ins);
|
||||
}
|
||||
|
||||
static void Log_Write_RC(void)
|
||||
void Plane::Log_Write_RC(void)
|
||||
{
|
||||
DataFlash.Log_Write_RCIN();
|
||||
DataFlash.Log_Write_RCOUT();
|
||||
}
|
||||
|
||||
static void Log_Write_Baro(void)
|
||||
void Plane::Log_Write_Baro(void)
|
||||
{
|
||||
DataFlash.Log_Write_Baro(barometer);
|
||||
}
|
||||
|
||||
// Write a AIRSPEED packet
|
||||
static void Log_Write_Airspeed(void)
|
||||
void Plane::Log_Write_Airspeed(void)
|
||||
{
|
||||
DataFlash.Log_Write_Airspeed(airspeed);
|
||||
}
|
||||
|
|
@ -492,7 +483,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
|
|||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
// Read the DataFlash.log memory : Packet Parser
|
||||
static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
|
||||
void Plane::Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
|
||||
"\nFree RAM: %u\n"),
|
||||
|
|
@ -501,13 +492,13 @@ static void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page)
|
|||
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
|
||||
|
||||
DataFlash.LogReadProcess(log_num, start_page, end_page,
|
||||
print_flight_mode,
|
||||
AP_HAL_MEMBERPROC(&Plane::print_flight_mode),
|
||||
cliSerial);
|
||||
}
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
// start a new log
|
||||
static void start_logging()
|
||||
void Plane::start_logging()
|
||||
{
|
||||
DataFlash.StartNewLog();
|
||||
DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING));
|
||||
|
|
@ -522,31 +513,29 @@ static void start_logging()
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
initialise logging subsystem
|
||||
*/
|
||||
void Plane::log_init(void)
|
||||
{
|
||||
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
|
||||
if (!DataFlash.CardInserted()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
|
||||
g.log_bitmask.set(0);
|
||||
} else if (DataFlash.NeedErase()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
|
||||
do_erase_logs();
|
||||
for (uint8_t i=0; i<num_gcs; i++) {
|
||||
gcs[i].reset_cli_timeout();
|
||||
}
|
||||
}
|
||||
arming.set_logging_available(DataFlash.CardInserted());
|
||||
}
|
||||
|
||||
#else // LOGGING_ENABLED
|
||||
|
||||
// dummy functions
|
||||
static void Log_Write_Startup(uint8_t type) {}
|
||||
static void Log_Write_EntireMission() {}
|
||||
static void Log_Write_Current() {}
|
||||
static void Log_Write_Nav_Tuning() {}
|
||||
static void Log_Write_TECS_Tuning() {}
|
||||
static void Log_Write_Performance() {}
|
||||
static void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd) {}
|
||||
static void Log_Write_Attitude() {}
|
||||
static void Log_Write_Control_Tuning() {}
|
||||
static void Log_Write_GPS(uint8_t instance) {}
|
||||
static void Log_Write_IMU() {}
|
||||
static void Log_Write_RC() {}
|
||||
static void Log_Write_Airspeed(void) {}
|
||||
static void Log_Write_Baro(void) {}
|
||||
static void Log_Write_Status() {}
|
||||
static void Log_Write_Sonar() {}
|
||||
#if OPTFLOW == ENABLED
|
||||
static void Log_Write_Optflow() {}
|
||||
#endif
|
||||
static void Log_Arm_Disarm() {}
|
||||
|
||||
static int8_t process_logs(uint8_t argc, const Menu::arg *argv) {
|
||||
int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -1,17 +1,19 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
* ArduPlane parameter definitions
|
||||
*
|
||||
*/
|
||||
|
||||
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} }
|
||||
#define ASCALAR(v, name, def) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.v, {def_value : def} }
|
||||
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info : class::var_info} }
|
||||
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info : class::var_info} }
|
||||
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
|
||||
#define GSCALAR(v, name, def) { plane.g.v.vtype, name, Parameters::k_param_ ## v, &plane.g.v, {def_value : def} }
|
||||
#define ASCALAR(v, name, def) { plane.aparm.v.vtype, name, Parameters::k_param_ ## v, &plane.aparm.v, {def_value : def} }
|
||||
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &plane.g.v, {group_info : class::var_info} }
|
||||
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &plane.v, {group_info : class::var_info} }
|
||||
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &plane.v, {group_info : class::var_info} }
|
||||
|
||||
const AP_Param::Info var_info[] PROGMEM = {
|
||||
const AP_Param::Info Plane::var_info[] PROGMEM = {
|
||||
// @Param: FORMAT_VERSION
|
||||
// @DisplayName: Eeprom format version number
|
||||
// @Description: This value is incremented when changes are made to the eeprom format
|
||||
|
|
@ -1224,7 +1226,7 @@ const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
|
|||
{ Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" },
|
||||
};
|
||||
|
||||
static void load_parameters(void)
|
||||
void Plane::load_parameters(void)
|
||||
{
|
||||
if (!AP_Param::check_var_info()) {
|
||||
cliSerial->printf_P(PSTR("Bad parameter table\n"));
|
||||
|
|
@ -1241,10 +1243,10 @@ static void load_parameters(void)
|
|||
g.format_version.set_and_save(Parameters::k_format_version);
|
||||
cliSerial->println_P(PSTR("done."));
|
||||
} else {
|
||||
uint32_t before = micros();
|
||||
uint32_t before = hal.scheduler->micros();
|
||||
// Load all auto-loaded EEPROM variables
|
||||
AP_Param::load_all();
|
||||
AP_Param::convert_old_parameters(&conversion_table[0], sizeof(conversion_table)/sizeof(conversion_table[0]));
|
||||
cliSerial->printf_P(PSTR("load_all took %luus\n"), micros() - before);
|
||||
cliSerial->printf_P(PSTR("load_all took %luus\n"), hal.scheduler->micros() - before);
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,93 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
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/>.
|
||||
*/
|
||||
/*
|
||||
constructor for main Plane class
|
||||
*/
|
||||
|
||||
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
|
||||
|
||||
Plane::Plane(void) :
|
||||
ins_sample_rate(AP_InertialSensor::RATE_50HZ),
|
||||
#if defined(HAL_BOARD_LOG_DIRECTORY)
|
||||
DataFlash(HAL_BOARD_LOG_DIRECTORY),
|
||||
#endif
|
||||
flight_modes(&g.flight_mode1),
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
ahrs(ins, barometer, gps, rng),
|
||||
#else
|
||||
ahrs(ins, barometer, gps),
|
||||
#endif
|
||||
L1_controller(ahrs),
|
||||
TECS_controller(ahrs, aparm),
|
||||
rollController(ahrs, aparm, DataFlash),
|
||||
pitchController(ahrs, aparm, DataFlash),
|
||||
yawController(ahrs, aparm),
|
||||
steerController(ahrs),
|
||||
num_gcs(MAVLINK_COMM_NUM_BUFFERS),
|
||||
nav_controller(&L1_controller),
|
||||
SpdHgt_Controller(&TECS_controller),
|
||||
ServoRelayEvents(relay),
|
||||
#if CAMERA == ENABLED
|
||||
camera(&relay),
|
||||
#endif
|
||||
rally(ahrs),
|
||||
control_mode(INITIALISING),
|
||||
previous_mode(INITIALISING),
|
||||
oldSwitchPosition(254),
|
||||
ground_start_count(5),
|
||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||
frsky_telemetry(ahrs, battery),
|
||||
#endif
|
||||
airspeed(aparm),
|
||||
flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL),
|
||||
aerodynamic_load_factor(1.0f),
|
||||
mission(ahrs,
|
||||
AP_HAL_MEMBERPROC(&Plane::start_command_callback),
|
||||
AP_HAL_MEMBERPROC(&Plane::verify_command_callback),
|
||||
AP_HAL_MEMBERPROC(&Plane::exit_mission_callback)),
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
terrain(ahrs, mission, rally),
|
||||
#endif
|
||||
#if OBC_FAILSAFE == ENABLED
|
||||
obc(mission, barometer, gps, rcmap),
|
||||
#endif
|
||||
home(ahrs.get_home()),
|
||||
G_Dt(0.02f),
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount(ahrs, current_loc),
|
||||
#endif
|
||||
arming(ahrs, barometer, compass, home_is_set, AP_HAL_MEMBERPROC(&Plane::gcs_send_text_P)),
|
||||
param_loader(var_info)
|
||||
{
|
||||
elevon.trim1 = 1500;
|
||||
elevon.trim2 = 1500;
|
||||
elevon.ch1_temp = 1500;
|
||||
elevon.ch2_temp = 1500;
|
||||
|
||||
steer_state.hold_course_cd = -1;
|
||||
steer_state.locked_course = false;
|
||||
steer_state.locked_course_err = 0;
|
||||
|
||||
auto_state.takeoff_complete = true;
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.no_crosstrack = true;
|
||||
auto_state.next_turn_angle = 90.0f;
|
||||
}
|
||||
|
||||
Plane plane;
|
||||
|
|
@ -0,0 +1,927 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define THISFIRMWARE "ArduPlane V3.3.0beta2"
|
||||
/*
|
||||
Lead developer: Andrew Tridgell
|
||||
|
||||
Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler, Amilcar Lucas, Gregory Fletcher, Paul Riseborough, Brandon Jones, Jon Challinger
|
||||
Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier, Yury MonZon
|
||||
|
||||
Please contribute your ideas! See http://dev.ardupilot.com for details
|
||||
|
||||
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/>.
|
||||
*/
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Header includes
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
#include <math.h>
|
||||
#include <stdarg.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_HAL.h>
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Progmem.h>
|
||||
#include <AP_Menu.h>
|
||||
#include <AP_Param.h>
|
||||
#include <StorageManager.h>
|
||||
#include <AP_GPS.h> // ArduPilot GPS library
|
||||
#include <AP_Baro.h> // ArduPilot barometer library
|
||||
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
|
||||
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
|
||||
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
|
||||
#include <AP_ADC_AnalogSource.h>
|
||||
#include <AP_InertialSensor.h> // Inertial Sensor Library
|
||||
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
|
||||
#include <RC_Channel.h> // RC Channel Library
|
||||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <Filter.h> // Filter library
|
||||
#include <AP_Buffer.h> // APM FIFO Buffer
|
||||
#include <AP_Relay.h> // APM relay
|
||||
#include <AP_Camera.h> // Photo or video camera
|
||||
#include <AP_Airspeed.h>
|
||||
#include <AP_Terrain.h>
|
||||
|
||||
#include <APM_OBC.h>
|
||||
#include <APM_Control.h>
|
||||
#include <AP_AutoTune.h>
|
||||
#include <GCS.h>
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
#include <AP_SerialManager.h> // Serial manager library
|
||||
#include <AP_Mount.h> // Camera/Antenna mount
|
||||
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
|
||||
#include <DataFlash.h>
|
||||
#include <SITL.h>
|
||||
#include <AP_Scheduler.h> // main loop scheduler
|
||||
|
||||
#include <AP_Navigation.h>
|
||||
#include <AP_L1_Control.h>
|
||||
#include <AP_RCMapper.h> // RC input mapping library
|
||||
|
||||
#include <AP_Vehicle.h>
|
||||
#include <AP_SpdHgtControl.h>
|
||||
#include <AP_TECS.h>
|
||||
#include <AP_NavEKF.h>
|
||||
#include <AP_Mission.h> // Mission command library
|
||||
|
||||
#include <AP_Notify.h> // Notify library
|
||||
#include <AP_BattMonitor.h> // Battery monitor library
|
||||
|
||||
#include <AP_Arming.h>
|
||||
#include <AP_BoardConfig.h>
|
||||
#include <AP_Frsky_Telem.h>
|
||||
#include <AP_ServoRelayEvents.h>
|
||||
|
||||
#include <AP_Rally.h>
|
||||
|
||||
#include <AP_OpticalFlow.h> // Optical Flow library
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
|
||||
// Local modules
|
||||
#include "defines.h"
|
||||
|
||||
#include "Parameters.h"
|
||||
|
||||
#include <AP_HAL_AVR.h>
|
||||
#include <AP_HAL_SITL.h>
|
||||
#include <AP_HAL_PX4.h>
|
||||
#include <AP_HAL_FLYMAPLE.h>
|
||||
#include <AP_HAL_Linux.h>
|
||||
#include <AP_HAL_Empty.h>
|
||||
#include <AP_HAL_VRBRAIN.h>
|
||||
|
||||
class Plane {
|
||||
public:
|
||||
friend class GCS_MAVLINK;
|
||||
friend class Parameters;
|
||||
|
||||
Plane(void);
|
||||
void setup();
|
||||
void loop();
|
||||
|
||||
private:
|
||||
// key aircraft parameters passed to multiple libraries
|
||||
AP_Vehicle::FixedWing aparm;
|
||||
AP_HAL::BetterStream* cliSerial;
|
||||
|
||||
// the rate we run the main loop
|
||||
const AP_InertialSensor::Sample_rate ins_sample_rate;
|
||||
|
||||
// Global parameters are all contained within the 'g' class.
|
||||
Parameters g;
|
||||
|
||||
// main loop scheduler
|
||||
AP_Scheduler scheduler;
|
||||
|
||||
// mapping between input channels
|
||||
RCMapper rcmap;
|
||||
|
||||
// board specific config
|
||||
AP_BoardConfig BoardConfig;
|
||||
|
||||
// primary control channels
|
||||
RC_Channel *channel_roll;
|
||||
RC_Channel *channel_pitch;
|
||||
RC_Channel *channel_throttle;
|
||||
RC_Channel *channel_rudder;
|
||||
|
||||
// notification object for LEDs, buzzers etc (parameter set to false disables external leds)
|
||||
AP_Notify notify;
|
||||
|
||||
// DataFlash
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
DataFlash_APM1 DataFlash;
|
||||
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
|
||||
DataFlash_APM2 DataFlash;
|
||||
#elif defined(HAL_BOARD_LOG_DIRECTORY)
|
||||
DataFlash_File DataFlash;
|
||||
#else
|
||||
// no dataflash driver
|
||||
DataFlash_Empty DataFlash;
|
||||
#endif
|
||||
|
||||
// has a log download started?
|
||||
bool in_log_download;
|
||||
|
||||
// scaled roll limit based on pitch
|
||||
int32_t roll_limit_cd;
|
||||
int32_t pitch_limit_min_cd;
|
||||
|
||||
// Sensors
|
||||
AP_GPS gps;
|
||||
RangeFinder rng;
|
||||
|
||||
// flight modes convenience array
|
||||
AP_Int8 *flight_modes;
|
||||
|
||||
AP_Baro barometer;
|
||||
Compass compass;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
AP_ADC_ADS7844 apm1_adc;
|
||||
#endif
|
||||
|
||||
AP_InertialSensor ins;
|
||||
|
||||
// Inertial Navigation EKF
|
||||
#if AP_AHRS_NAVEKF_AVAILABLE
|
||||
AP_AHRS_NavEKF ahrs;
|
||||
#else
|
||||
AP_AHRS_DCM ahrs;
|
||||
#endif
|
||||
|
||||
AP_L1_Control L1_controller;
|
||||
AP_TECS TECS_controller;
|
||||
|
||||
// Attitude to servo controllers
|
||||
AP_RollController rollController;
|
||||
AP_PitchController pitchController;
|
||||
AP_YawController yawController;
|
||||
AP_SteerController steerController;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
SITL sitl;
|
||||
#endif
|
||||
|
||||
// Training mode
|
||||
bool training_manual_roll; // user has manual roll control
|
||||
bool training_manual_pitch; // user has manual pitch control
|
||||
|
||||
/*
|
||||
keep steering and rudder control separated until we update servos,
|
||||
to allow for a separate wheel servo from rudder servo
|
||||
*/
|
||||
struct {
|
||||
bool ground_steering; // are we doing ground steering?
|
||||
int16_t steering; // value for nose/tail wheel
|
||||
int16_t rudder; // value for rudder
|
||||
} steering_control;
|
||||
|
||||
// should throttle be pass-thru in guided?
|
||||
bool guided_throttle_passthru;
|
||||
|
||||
// are we doing calibration? This is used to allow heartbeat to
|
||||
// external failsafe boards during baro and airspeed calibration
|
||||
bool in_calibration;
|
||||
|
||||
|
||||
// GCS selection
|
||||
AP_SerialManager serial_manager;
|
||||
const uint8_t num_gcs;
|
||||
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
|
||||
|
||||
// selected navigation controller
|
||||
AP_Navigation *nav_controller;
|
||||
|
||||
// selected navigation controller
|
||||
AP_SpdHgtControl *SpdHgt_Controller;
|
||||
|
||||
// Analog Inputs
|
||||
// a pin for reading the receiver RSSI voltage.
|
||||
AP_HAL::AnalogSource *rssi_analog_source;
|
||||
|
||||
// rangefinder
|
||||
RangeFinder rangefinder;
|
||||
|
||||
struct {
|
||||
bool in_range;
|
||||
float correction;
|
||||
uint32_t last_correction_time_ms;
|
||||
uint8_t in_range_count;
|
||||
} rangefinder_state;
|
||||
|
||||
// Relay
|
||||
AP_Relay relay;
|
||||
|
||||
// handle servo and relay events
|
||||
AP_ServoRelayEvents ServoRelayEvents;
|
||||
|
||||
// Camera
|
||||
#if CAMERA == ENABLED
|
||||
AP_Camera camera;
|
||||
#endif
|
||||
|
||||
// Optical flow sensor
|
||||
OpticalFlow optflow;
|
||||
|
||||
// Rally Ponints
|
||||
AP_Rally rally;
|
||||
|
||||
// remember if USB is connected, so we can adjust baud rate
|
||||
bool usb_connected;
|
||||
|
||||
// This is the state of the flight control system
|
||||
// There are multiple states defined such as MANUAL, FBW-A, AUTO
|
||||
enum FlightMode control_mode;
|
||||
enum FlightMode previous_mode;
|
||||
|
||||
// Used to maintain the state of the previous control switch position
|
||||
// This is set to 254 when we need to re-read the switch
|
||||
uint8_t oldSwitchPosition;
|
||||
|
||||
// This is used to enable the inverted flight feature
|
||||
bool inverted_flight;
|
||||
|
||||
// This is used to enable the PX4IO override for testing
|
||||
bool px4io_override_enabled;
|
||||
|
||||
struct {
|
||||
// These are trim values used for elevon control
|
||||
// For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are
|
||||
// equivalent aileron and elevator, not left and right elevon
|
||||
uint16_t trim1;
|
||||
uint16_t trim2;
|
||||
// These are used in the calculation of elevon1_trim and elevon2_trim
|
||||
uint16_t ch1_temp;
|
||||
uint16_t ch2_temp;
|
||||
} elevon;
|
||||
|
||||
// Failsafe
|
||||
struct {
|
||||
// Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold
|
||||
// RC receiver should be set up to output a low throttle value when signal is lost
|
||||
uint8_t ch3_failsafe:1;
|
||||
|
||||
// has the saved mode for failsafe been set?
|
||||
uint8_t saved_mode_set:1;
|
||||
|
||||
// flag to hold whether battery low voltage threshold has been breached
|
||||
uint8_t low_battery:1;
|
||||
|
||||
// saved flight mode
|
||||
enum FlightMode saved_mode;
|
||||
|
||||
// A tracking variable for type of failsafe active
|
||||
// Used for failsafe based on loss of RC signal or GCS signal
|
||||
int16_t state;
|
||||
|
||||
// number of low ch3 values
|
||||
uint8_t ch3_counter;
|
||||
|
||||
// the time when the last HEARTBEAT message arrived from a GCS
|
||||
uint32_t last_heartbeat_ms;
|
||||
|
||||
// A timer used to track how long we have been in a "short failsafe" condition due to loss of RC signal
|
||||
uint32_t ch3_timer_ms;
|
||||
|
||||
uint32_t last_valid_rc_ms;
|
||||
} failsafe;
|
||||
|
||||
// A counter used to count down valid gps fixes to allow the gps estimate to settle
|
||||
// before recording our home position (and executing a ground start if we booted with an air start)
|
||||
uint8_t ground_start_count;
|
||||
|
||||
// true if we have a position estimate from AHRS
|
||||
bool have_position;
|
||||
|
||||
// Airspeed
|
||||
// The calculated airspeed to use in FBW-B. Also used in higher modes for insuring min ground speed is met.
|
||||
// Also used for flap deployment criteria. Centimeters per second.
|
||||
int32_t target_airspeed_cm;
|
||||
|
||||
// The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second.
|
||||
float airspeed_error_cm;
|
||||
|
||||
// An amount that the airspeed should be increased in auto modes based on the user positioning the
|
||||
// throttle stick in the top half of the range. Centimeters per second.
|
||||
int16_t airspeed_nudge_cm;
|
||||
|
||||
// Similar to airspeed_nudge, but used when no airspeed sensor.
|
||||
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
int16_t throttle_nudge;
|
||||
|
||||
// receiver RSSI
|
||||
uint8_t receiver_rssi;
|
||||
|
||||
// Ground speed
|
||||
// The amount current ground speed is below min ground speed. Centimeters per second
|
||||
int32_t groundspeed_undershoot;
|
||||
|
||||
// Difference between current altitude and desired altitude. Centimeters
|
||||
int32_t altitude_error_cm;
|
||||
|
||||
// Battery Sensors
|
||||
AP_BattMonitor battery;
|
||||
|
||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||
// FrSky telemetry support
|
||||
AP_Frsky_Telem frsky_telemetry;
|
||||
#endif
|
||||
|
||||
// Airspeed Sensors
|
||||
AP_Airspeed airspeed;
|
||||
|
||||
// ACRO controller state
|
||||
struct {
|
||||
bool locked_roll;
|
||||
bool locked_pitch;
|
||||
float locked_roll_err;
|
||||
int32_t locked_pitch_cd;
|
||||
} acro_state;
|
||||
|
||||
// CRUISE controller state
|
||||
struct {
|
||||
bool locked_heading;
|
||||
int32_t locked_heading_cd;
|
||||
uint32_t lock_timer_ms;
|
||||
} cruise_state;
|
||||
|
||||
// ground steering controller state
|
||||
struct {
|
||||
// Direction held during phases of takeoff and landing centidegrees
|
||||
// A value of -1 indicates the course has not been set/is not in use
|
||||
// this is a 0..36000 value, or -1 for disabled
|
||||
int32_t hold_course_cd;
|
||||
|
||||
// locked_course and locked_course_cd are used in stabilize mode
|
||||
// when ground steering is active, and for steering in auto-takeoff
|
||||
bool locked_course;
|
||||
float locked_course_err;
|
||||
} steer_state;
|
||||
|
||||
// flight mode specific
|
||||
struct {
|
||||
// Flag for using gps ground course instead of INS yaw. Set false when takeoff command in process.
|
||||
bool takeoff_complete:1;
|
||||
|
||||
// Flag to indicate if we have landed.
|
||||
// Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
|
||||
bool land_complete:1;
|
||||
|
||||
// should we fly inverted?
|
||||
bool inverted_flight:1;
|
||||
|
||||
// should we disable cross-tracking for the next waypoint?
|
||||
bool next_wp_no_crosstrack:1;
|
||||
|
||||
// should we use cross-tracking for this waypoint?
|
||||
bool no_crosstrack:1;
|
||||
|
||||
// in FBWA taildragger takeoff mode
|
||||
bool fbwa_tdrag_takeoff_mode:1;
|
||||
|
||||
// have we checked for an auto-land?
|
||||
bool checked_for_autoland:1;
|
||||
|
||||
// denotes if a go-around has been commanded for landing
|
||||
bool commanded_go_around:1;
|
||||
|
||||
// Altitude threshold to complete a takeoff command in autonomous
|
||||
// modes. Centimeters above home
|
||||
int32_t takeoff_altitude_rel_cm;
|
||||
|
||||
// Minimum pitch to hold during takeoff command execution. Hundredths of a degree
|
||||
int16_t takeoff_pitch_cd;
|
||||
|
||||
// the highest airspeed we have reached since entering AUTO. Used
|
||||
// to control ground takeoff
|
||||
float highest_airspeed;
|
||||
|
||||
// initial pitch. Used to detect if nose is rising in a tail dragger
|
||||
int16_t initial_pitch_cd;
|
||||
|
||||
// turn angle for next leg of mission
|
||||
float next_turn_angle;
|
||||
|
||||
// filtered sink rate for landing
|
||||
float land_sink_rate;
|
||||
|
||||
// time when we first pass min GPS speed on takeoff
|
||||
uint32_t takeoff_speed_time_ms;
|
||||
|
||||
// distance to next waypoint
|
||||
float wp_distance;
|
||||
|
||||
// proportion to next waypoint
|
||||
float wp_proportion;
|
||||
|
||||
// last time is_flying() returned true in milliseconds
|
||||
uint32_t last_flying_ms;
|
||||
} auto_state;
|
||||
|
||||
// true if we are in an auto-throttle mode, which means
|
||||
// we need to run the speed/height controller
|
||||
bool auto_throttle_mode;
|
||||
|
||||
// this controls throttle suppression in auto modes
|
||||
bool throttle_suppressed;
|
||||
|
||||
AP_SpdHgtControl::FlightStage flight_stage;
|
||||
|
||||
// probability of aircraft is currently in flight. range from 0 to
|
||||
// 1 where 1 is 100% sure we're in flight
|
||||
float isFlyingProbability;
|
||||
|
||||
// Navigation control variables
|
||||
// The instantaneous desired bank angle. Hundredths of a degree
|
||||
int32_t nav_roll_cd;
|
||||
|
||||
// The instantaneous desired pitch angle. Hundredths of a degree
|
||||
int32_t nav_pitch_cd;
|
||||
|
||||
// we separate out rudder input to allow for RUDDER_ONLY=1
|
||||
int16_t rudder_input;
|
||||
|
||||
// the aerodymamic load factor. This is calculated from the demanded
|
||||
// roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
|
||||
float aerodynamic_load_factor;
|
||||
|
||||
// a smoothed airspeed estimate, used for limiting roll angle
|
||||
float smoothed_airspeed;
|
||||
|
||||
// Mission library
|
||||
AP_Mission mission;
|
||||
|
||||
// terrain handling
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
AP_Terrain terrain;
|
||||
#endif
|
||||
|
||||
// Outback Challenge Failsafe Support
|
||||
#if OBC_FAILSAFE == ENABLED
|
||||
APM_OBC obc;
|
||||
#endif
|
||||
|
||||
/*
|
||||
meta data to support counting the number of circles in a loiter
|
||||
*/
|
||||
struct {
|
||||
// previous target bearing, used to update sum_cd
|
||||
int32_t old_target_bearing_cd;
|
||||
|
||||
// Total desired rotation in a loiter. Used for Loiter Turns commands.
|
||||
int32_t total_cd;
|
||||
|
||||
// total angle completed in the loiter so far
|
||||
int32_t sum_cd;
|
||||
|
||||
// Direction for loiter. 1 for clockwise, -1 for counter-clockwise
|
||||
int8_t direction;
|
||||
|
||||
// start time of the loiter. Milliseconds.
|
||||
uint32_t start_time_ms;
|
||||
|
||||
// The amount of time we should stay in a loiter for the Loiter Time command. Milliseconds.
|
||||
uint32_t time_max_ms;
|
||||
} loiter;
|
||||
|
||||
|
||||
// Conditional command
|
||||
// A value used in condition commands (eg delay, change alt, etc.)
|
||||
// For example in a change altitude command, it is the altitude to change to.
|
||||
int32_t condition_value;
|
||||
|
||||
// Sometimes there is a second condition required:
|
||||
int32_t condition_value2;
|
||||
// A starting value used to check the status of a conditional command.
|
||||
// For example in a delay command the condition_start records that start time for the delay
|
||||
uint32_t condition_start;
|
||||
// A value used in condition commands. For example the rate at which to change altitude.
|
||||
int16_t condition_rate;
|
||||
|
||||
// 3D Location vectors
|
||||
// Location structure defined in AP_Common
|
||||
const struct Location &home;
|
||||
|
||||
// Flag for if we have g_gps lock and have set the home location in AHRS
|
||||
enum HomeState home_is_set;
|
||||
|
||||
// The location of the previous waypoint. Used for track following and altitude ramp calculations
|
||||
Location prev_WP_loc;
|
||||
|
||||
// The plane's current location
|
||||
struct Location current_loc;
|
||||
|
||||
// The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations.
|
||||
Location next_WP_loc;
|
||||
|
||||
// The location of the active waypoint in Guided mode.
|
||||
struct Location guided_WP_loc;
|
||||
|
||||
// special purpose command used only after mission completed to return vehicle to home or rally point
|
||||
struct AP_Mission::Mission_Command auto_rtl_command;
|
||||
|
||||
// Altitude control
|
||||
struct {
|
||||
// target altitude above sea level in cm. Used for barometric
|
||||
// altitude navigation
|
||||
int32_t amsl_cm;
|
||||
|
||||
// Altitude difference between previous and current waypoint in
|
||||
// centimeters. Used for glide slope handling
|
||||
int32_t offset_cm;
|
||||
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
// are we trying to follow terrain?
|
||||
bool terrain_following;
|
||||
|
||||
// target altitude above terrain in cm, valid if terrain_following
|
||||
// is set
|
||||
int32_t terrain_alt_cm;
|
||||
|
||||
// lookahead value for height error reporting
|
||||
float lookahead;
|
||||
#endif
|
||||
} target_altitude;
|
||||
|
||||
// INS variables
|
||||
// The main loop execution time. Seconds
|
||||
// This is the time between calls to the DCM algorithm and is the Integration time for the gyros.
|
||||
float G_Dt;
|
||||
|
||||
// Performance monitoring
|
||||
// Timer used to accrue data and trigger recording of the performanc monitoring log message
|
||||
uint32_t perf_mon_timer;
|
||||
|
||||
// The maximum and minimum main loop execution time recorded in the current performance monitoring interval
|
||||
uint32_t G_Dt_max;
|
||||
uint32_t G_Dt_min;
|
||||
|
||||
// System Timers
|
||||
// Time in microseconds of start of main control loop
|
||||
uint32_t fast_loopTimer_us;
|
||||
|
||||
// Number of milliseconds used in last main loop cycle
|
||||
uint32_t delta_us_fast_loop;
|
||||
|
||||
// Counter of main loop executions. Used for performance monitoring and failsafe processing
|
||||
uint16_t mainLoop_count;
|
||||
|
||||
// Camera/Antenna mount tracking and stabilisation stuff
|
||||
#if MOUNT == ENABLED
|
||||
// current_loc uses the baro/gps soloution for altitude rather than gps only.
|
||||
AP_Mount camera_mount;
|
||||
#endif
|
||||
|
||||
// Arming/Disarming mangement class
|
||||
AP_Arming arming;
|
||||
|
||||
AP_Param param_loader;
|
||||
|
||||
static const AP_Scheduler::Task scheduler_tasks[];
|
||||
static const AP_Param::Info var_info[];
|
||||
|
||||
bool demoing_servos;
|
||||
|
||||
// use this to prevent recursion during sensor init
|
||||
bool in_mavlink_delay;
|
||||
|
||||
// true if we are out of time in our event timeslice
|
||||
bool gcs_out_of_time;
|
||||
|
||||
|
||||
void demo_servos(uint8_t i);
|
||||
void adjust_nav_pitch_throttle(void);
|
||||
void update_load_factor(void);
|
||||
void send_heartbeat(mavlink_channel_t chan);
|
||||
void send_attitude(mavlink_channel_t chan);
|
||||
void send_fence_status(mavlink_channel_t chan);
|
||||
void send_extended_status1(mavlink_channel_t chan);
|
||||
void send_location(mavlink_channel_t chan);
|
||||
void send_nav_controller_output(mavlink_channel_t chan);
|
||||
void send_servo_out(mavlink_channel_t chan);
|
||||
void send_radio_out(mavlink_channel_t chan);
|
||||
void send_vfr_hud(mavlink_channel_t chan);
|
||||
void send_simstate(mavlink_channel_t chan);
|
||||
void send_hwstatus(mavlink_channel_t chan);
|
||||
void send_wind(mavlink_channel_t chan);
|
||||
void send_rangefinder(mavlink_channel_t chan);
|
||||
void send_current_waypoint(mavlink_channel_t chan);
|
||||
void send_statustext(mavlink_channel_t chan);
|
||||
bool telemetry_delayed(mavlink_channel_t chan);
|
||||
void gcs_send_message(enum ap_message id);
|
||||
void gcs_data_stream_send(void);
|
||||
void gcs_update(void);
|
||||
void gcs_send_text_P(gcs_severity severity, const prog_char_t *str);
|
||||
void gcs_send_airspeed_calibration(const Vector3f &vg);
|
||||
void gcs_retry_deferred(void);
|
||||
void do_erase_logs(void);
|
||||
void Log_Write_Attitude(void);
|
||||
void Log_Write_Performance();
|
||||
void Log_Write_Startup(uint8_t type);
|
||||
void Log_Write_EntireMission();
|
||||
void Log_Write_Control_Tuning();
|
||||
void Log_Write_TECS_Tuning(void);
|
||||
void Log_Write_Nav_Tuning();
|
||||
void Log_Write_Status();
|
||||
void Log_Write_Sonar();
|
||||
void Log_Write_Optflow();
|
||||
void Log_Write_Current();
|
||||
void Log_Arm_Disarm();
|
||||
void Log_Write_GPS(uint8_t instance);
|
||||
void Log_Write_IMU();
|
||||
void Log_Write_RC(void);
|
||||
void Log_Write_Baro(void);
|
||||
void Log_Write_Airspeed(void);
|
||||
void Log_Read(uint16_t log_num, int16_t start_page, int16_t end_page);
|
||||
void start_logging();
|
||||
void load_parameters(void);
|
||||
void adjust_altitude_target();
|
||||
void setup_glide_slope(void);
|
||||
int32_t get_RTL_altitude();
|
||||
float relative_altitude(void);
|
||||
int32_t relative_altitude_abs_cm(void);
|
||||
void set_target_altitude_current(void);
|
||||
void set_target_altitude_current_adjusted(void);
|
||||
void set_target_altitude_location(const Location &loc);
|
||||
int32_t relative_target_altitude_cm(void);
|
||||
void change_target_altitude(int32_t change_cm);
|
||||
void set_target_altitude_proportion(const Location &loc, float proportion);
|
||||
void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
|
||||
int32_t calc_altitude_error_cm(void);
|
||||
void check_minimum_altitude(void);
|
||||
void reset_offset_altitude(void);
|
||||
void set_offset_altitude_location(const Location &loc);
|
||||
bool above_location_current(const Location &loc);
|
||||
void setup_terrain_target_alt(Location &loc);
|
||||
int32_t adjusted_altitude_cm(void);
|
||||
int32_t adjusted_relative_altitude_cm(void);
|
||||
float height_above_target(void);
|
||||
float lookahead_adjustment(void);
|
||||
float rangefinder_correction(void);
|
||||
void rangefinder_height_update(void);
|
||||
void add_altitude_data(unsigned long xl, long y);
|
||||
void set_next_WP(const struct Location &loc);
|
||||
void set_guided_WP(void);
|
||||
void init_home();
|
||||
void update_home();
|
||||
void do_RTL(void);
|
||||
bool verify_takeoff();
|
||||
bool verify_loiter_unlim();
|
||||
bool verify_loiter_time();
|
||||
bool verify_loiter_turns();
|
||||
bool verify_loiter_to_alt();
|
||||
bool verify_RTL();
|
||||
bool verify_continue_and_change_alt();
|
||||
bool verify_wait_delay();
|
||||
bool verify_change_alt();
|
||||
bool verify_within_distance();
|
||||
void do_loiter_at_location();
|
||||
void do_take_picture();
|
||||
void log_picture();
|
||||
void exit_mission_callback();
|
||||
void update_commands(void);
|
||||
void mavlink_delay(uint32_t ms);
|
||||
void read_control_switch();
|
||||
uint8_t readSwitch(void);
|
||||
void reset_control_switch();
|
||||
void autotune_start(void);
|
||||
void autotune_restore(void);
|
||||
bool fly_inverted(void);
|
||||
void failsafe_short_on_event(enum failsafe_state fstype);
|
||||
void failsafe_long_on_event(enum failsafe_state fstype);
|
||||
void failsafe_short_off_event();
|
||||
void low_battery_event(void);
|
||||
void update_events(void);
|
||||
uint8_t max_fencepoints(void);
|
||||
Vector2l get_fence_point_with_index(unsigned i);
|
||||
void set_fence_point_with_index(Vector2l &point, unsigned i);
|
||||
void geofence_load(void);
|
||||
bool geofence_present(void);
|
||||
void geofence_update_pwm_enabled_state();
|
||||
bool geofence_set_enabled(bool enable, GeofenceEnableReason r);
|
||||
bool geofence_enabled(void);
|
||||
bool geofence_set_floor_enabled(bool floor_enable);
|
||||
bool geofence_check_minalt(void);
|
||||
bool geofence_check_maxalt(void);
|
||||
void geofence_check(bool altitude_check_only);
|
||||
bool geofence_stickmixing(void);
|
||||
void geofence_send_status(mavlink_channel_t chan);
|
||||
bool geofence_breached(void);
|
||||
bool verify_land();
|
||||
void disarm_if_autoland_complete();
|
||||
void setup_landing_glide_slope(void);
|
||||
bool jump_to_landing_sequence(void);
|
||||
float tecs_hgt_afe(void);
|
||||
void set_nav_controller(void);
|
||||
void loiter_angle_reset(void);
|
||||
void loiter_angle_update(void);
|
||||
void navigate();
|
||||
void calc_airspeed_errors();
|
||||
void calc_gndspeed_undershoot();
|
||||
void update_loiter();
|
||||
void update_cruise();
|
||||
void update_fbwb_speed_height(void);
|
||||
void setup_turn_angle(void);
|
||||
bool create_mixer_file(const char *filename);
|
||||
bool setup_failsafe_mixing(void);
|
||||
void set_control_channels(void);
|
||||
void init_rc_in();
|
||||
void init_rc_out();
|
||||
void rudder_arm_check();
|
||||
void read_radio();
|
||||
void control_failsafe(uint16_t pwm);
|
||||
void trim_control_surfaces();
|
||||
void trim_radio();
|
||||
bool rc_failsafe_active(void);
|
||||
void init_barometer(void);
|
||||
void init_rangefinder(void);
|
||||
void read_rangefinder(void);
|
||||
void read_airspeed(void);
|
||||
void zero_airspeed(bool in_startup);
|
||||
void read_battery(void);
|
||||
void read_receiver_rssi(void);
|
||||
void report_radio();
|
||||
void report_ins();
|
||||
void report_compass();
|
||||
void print_radio_values();
|
||||
void print_done();
|
||||
void print_blanks(int16_t num);
|
||||
void print_divider(void);
|
||||
void zero_eeprom(void);
|
||||
void print_enabled(bool b);
|
||||
void print_accel_offsets_and_scaling(void);
|
||||
void print_gyro_offsets(void);
|
||||
void init_ardupilot();
|
||||
void startup_ground(void);
|
||||
enum FlightMode get_previous_mode();
|
||||
void set_mode(enum FlightMode mode);
|
||||
bool mavlink_set_mode(uint8_t mode);
|
||||
void exit_mode(enum FlightMode mode);
|
||||
void check_long_failsafe();
|
||||
void check_short_failsafe();
|
||||
void startup_INS_ground(void);
|
||||
void update_notify();
|
||||
void resetPerfData(void);
|
||||
void check_usb_mux(void);
|
||||
void print_comma(void);
|
||||
void servo_write(uint8_t ch, uint16_t pwm);
|
||||
bool should_log(uint32_t mask);
|
||||
void frsky_telemetry_send(void);
|
||||
uint8_t throttle_percentage(void);
|
||||
void change_arm_state(void);
|
||||
bool disarm_motors(void);
|
||||
bool arm_motors(AP_Arming::ArmingMethod method);
|
||||
bool auto_takeoff_check(void);
|
||||
void takeoff_calc_roll(void);
|
||||
void takeoff_calc_pitch(void);
|
||||
int8_t takeoff_tail_hold(void);
|
||||
void print_hit_enter();
|
||||
void ahrs_update();
|
||||
void update_speed_height(void);
|
||||
void update_GPS_50Hz(void);
|
||||
void update_GPS_10Hz(void);
|
||||
void update_compass(void);
|
||||
void update_alt(void);
|
||||
void obc_fs_check(void);
|
||||
void compass_accumulate(void);
|
||||
void barometer_accumulate(void);
|
||||
void update_optical_flow(void);
|
||||
void one_second_loop(void);
|
||||
void airspeed_ratio_update(void);
|
||||
void update_mount(void);
|
||||
void log_perf_info(void);
|
||||
void compass_save(void);
|
||||
void update_logging1(void);
|
||||
void update_logging2(void);
|
||||
void terrain_update(void);
|
||||
void update_flight_mode(void);
|
||||
void stabilize();
|
||||
void set_servos();
|
||||
void update_aux();
|
||||
void determine_is_flying(void);
|
||||
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
|
||||
void handle_auto_mode(void);
|
||||
void calc_throttle();
|
||||
void calc_nav_roll();
|
||||
void calc_nav_pitch();
|
||||
void update_flight_stage();
|
||||
void update_navigation();
|
||||
void set_flight_stage(AP_SpdHgtControl::FlightStage fs);
|
||||
bool is_flying(void);
|
||||
float get_speed_scaler(void);
|
||||
bool stick_mixing_enabled(void);
|
||||
void stabilize_roll(float speed_scaler);
|
||||
void stabilize_pitch(float speed_scaler);
|
||||
void stick_mix_channel(RC_Channel *channel, int16_t &servo_out);
|
||||
void stabilize_stick_mixing_direct();
|
||||
void stabilize_stick_mixing_fbw();
|
||||
void stabilize_yaw(float speed_scaler);
|
||||
void stabilize_training(float speed_scaler);
|
||||
void stabilize_acro(float speed_scaler);
|
||||
void calc_nav_yaw_coordinated(float speed_scaler);
|
||||
void calc_nav_yaw_course(void);
|
||||
void calc_nav_yaw_ground(void);
|
||||
void throttle_slew_limit(int16_t last_throttle);
|
||||
void flap_slew_limit(int8_t &last_value, int8_t &new_value);
|
||||
bool suppress_throttle(void);
|
||||
void channel_output_mixer(uint8_t mixing_type, int16_t &chan1_out, int16_t &chan2_out);
|
||||
void flaperon_update(int8_t flap_percent);
|
||||
bool start_command(const AP_Mission::Mission_Command& cmd);
|
||||
bool verify_command(const AP_Mission::Mission_Command& cmd);
|
||||
void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void do_land(const AP_Mission::Mission_Command& cmd);
|
||||
void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
|
||||
void do_loiter_time(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);
|
||||
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);
|
||||
void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||
void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
bool start_command_callback(const AP_Mission::Mission_Command &cmd);
|
||||
bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
|
||||
void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd);
|
||||
void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
|
||||
void run_cli(AP_HAL::UARTDriver *port);
|
||||
void log_init();
|
||||
|
||||
public:
|
||||
void mavlink_delay_cb();
|
||||
void failsafe_check(void);
|
||||
bool print_log_menu(void);
|
||||
int8_t dump_log(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t select_logs(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t process_logs(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t setup_factory(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t setup_erase(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
void test_wp_print(const AP_Mission::Mission_Command& cmd);
|
||||
int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
|
||||
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
|
||||
};
|
||||
|
||||
#define THISFIRMWARE "ArduPlane V3.3.0beta2"
|
||||
|
||||
#define MENU_FUNC(func) AP_HAL_CLASSPROC(&plane, &Plane::func)
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
extern Plane plane;
|
||||
|
|
@ -14,6 +14,8 @@
|
|||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
altitude handling routines. These cope with both barometric control
|
||||
and terrain following control
|
||||
|
|
@ -22,7 +24,7 @@
|
|||
/*
|
||||
adjust altitude target depending on mode
|
||||
*/
|
||||
static void adjust_altitude_target()
|
||||
void Plane::adjust_altitude_target()
|
||||
{
|
||||
if (control_mode == FLY_BY_WIRE_B ||
|
||||
control_mode == CRUISE) {
|
||||
|
|
@ -55,7 +57,7 @@ static void adjust_altitude_target()
|
|||
/*
|
||||
setup for a gradual glide slope to the next waypoint, if appropriate
|
||||
*/
|
||||
static void setup_glide_slope(void)
|
||||
void Plane::setup_glide_slope(void)
|
||||
{
|
||||
// establish the distance we are travelling to the next waypoint,
|
||||
// for calculating out rate of change of altitude
|
||||
|
|
@ -102,7 +104,7 @@ static void setup_glide_slope(void)
|
|||
/*
|
||||
return RTL altitude as AMSL altitude
|
||||
*/
|
||||
static int32_t get_RTL_altitude()
|
||||
int32_t Plane::get_RTL_altitude()
|
||||
{
|
||||
if (g.RTL_altitude_cm < 0) {
|
||||
return current_loc.alt;
|
||||
|
|
@ -114,7 +116,7 @@ static int32_t get_RTL_altitude()
|
|||
/*
|
||||
return relative altitude in meters (relative to home)
|
||||
*/
|
||||
static float relative_altitude(void)
|
||||
float Plane::relative_altitude(void)
|
||||
{
|
||||
return (current_loc.alt - home.alt) * 0.01f;
|
||||
}
|
||||
|
|
@ -122,7 +124,7 @@ static float relative_altitude(void)
|
|||
/*
|
||||
return relative altitude in centimeters, absolute value
|
||||
*/
|
||||
static int32_t relative_altitude_abs_cm(void)
|
||||
int32_t Plane::relative_altitude_abs_cm(void)
|
||||
{
|
||||
return labs(current_loc.alt - home.alt);
|
||||
}
|
||||
|
|
@ -133,7 +135,7 @@ static int32_t relative_altitude_abs_cm(void)
|
|||
setting up for altitude hold, such as when releasing elevator in
|
||||
CRUISE mode.
|
||||
*/
|
||||
static void set_target_altitude_current(void)
|
||||
void Plane::set_target_altitude_current(void)
|
||||
{
|
||||
// record altitude above sea level at the current time as our
|
||||
// target altitude
|
||||
|
|
@ -160,7 +162,7 @@ static void set_target_altitude_current(void)
|
|||
/*
|
||||
set the target altitude to the current altitude, with ALT_OFFSET adjustment
|
||||
*/
|
||||
static void set_target_altitude_current_adjusted(void)
|
||||
void Plane::set_target_altitude_current_adjusted(void)
|
||||
{
|
||||
set_target_altitude_current();
|
||||
|
||||
|
|
@ -171,7 +173,7 @@ static void set_target_altitude_current_adjusted(void)
|
|||
/*
|
||||
set target altitude based on a location structure
|
||||
*/
|
||||
static void set_target_altitude_location(const Location &loc)
|
||||
void Plane::set_target_altitude_location(const Location &loc)
|
||||
{
|
||||
target_altitude.amsl_cm = loc.alt;
|
||||
if (loc.flags.relative_alt) {
|
||||
|
|
@ -201,7 +203,7 @@ static void set_target_altitude_location(const Location &loc)
|
|||
return relative to home target altitude in centimeters. Used for
|
||||
altitude control libraries
|
||||
*/
|
||||
static int32_t relative_target_altitude_cm(void)
|
||||
int32_t Plane::relative_target_altitude_cm(void)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
float relative_home_height;
|
||||
|
|
@ -230,7 +232,7 @@ static int32_t relative_target_altitude_cm(void)
|
|||
change the current target altitude by an amount in centimeters. Used
|
||||
to cope with changes due to elevator in CRUISE or FBWB
|
||||
*/
|
||||
static void change_target_altitude(int32_t change_cm)
|
||||
void Plane::change_target_altitude(int32_t change_cm)
|
||||
{
|
||||
target_altitude.amsl_cm += change_cm;
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
|
|
@ -251,7 +253,7 @@ static void change_target_altitude(int32_t change_cm)
|
|||
Note that target_altitude is setup initially based on the
|
||||
destination waypoint
|
||||
*/
|
||||
static void set_target_altitude_proportion(const Location &loc, float proportion)
|
||||
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
|
||||
{
|
||||
set_target_altitude_location(loc);
|
||||
proportion = constrain_float(proportion, 0.0f, 1.0f);
|
||||
|
|
@ -273,7 +275,7 @@ static void set_target_altitude_proportion(const Location &loc, float proportion
|
|||
constrain target altitude to be between two locations. Used to
|
||||
ensure we stay within two waypoints in altitude
|
||||
*/
|
||||
static void constrain_target_altitude_location(const Location &loc1, const Location &loc2)
|
||||
void Plane::constrain_target_altitude_location(const Location &loc1, const Location &loc2)
|
||||
{
|
||||
if (loc1.alt > loc2.alt) {
|
||||
target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.alt);
|
||||
|
|
@ -285,7 +287,7 @@ static void constrain_target_altitude_location(const Location &loc1, const Locat
|
|||
/*
|
||||
return error between target altitude and current altitude
|
||||
*/
|
||||
static int32_t calc_altitude_error_cm(void)
|
||||
int32_t Plane::calc_altitude_error_cm(void)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
float terrain_height;
|
||||
|
|
@ -300,7 +302,7 @@ static int32_t calc_altitude_error_cm(void)
|
|||
/*
|
||||
check for FBWB_min_altitude_cm violation
|
||||
*/
|
||||
static void check_minimum_altitude(void)
|
||||
void Plane::check_minimum_altitude(void)
|
||||
{
|
||||
if (g.FBWB_min_altitude_cm == 0) {
|
||||
return;
|
||||
|
|
@ -324,7 +326,7 @@ static void check_minimum_altitude(void)
|
|||
/*
|
||||
reset the altitude offset used for glide slopes
|
||||
*/
|
||||
static void reset_offset_altitude(void)
|
||||
void Plane::reset_offset_altitude(void)
|
||||
{
|
||||
target_altitude.offset_cm = 0;
|
||||
}
|
||||
|
|
@ -336,7 +338,7 @@ static void reset_offset_altitude(void)
|
|||
destination is above the current altitude then the result is
|
||||
positive.
|
||||
*/
|
||||
static void set_offset_altitude_location(const Location &loc)
|
||||
void Plane::set_offset_altitude_location(const Location &loc)
|
||||
{
|
||||
target_altitude.offset_cm = loc.alt - current_loc.alt;
|
||||
|
||||
|
|
@ -381,7 +383,7 @@ static void set_offset_altitude_location(const Location &loc)
|
|||
lower pressure altitude, if current_loc is in a low part of the
|
||||
terrain
|
||||
*/
|
||||
static bool above_location_current(const Location &loc)
|
||||
bool Plane::above_location_current(const Location &loc)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
float terrain_alt;
|
||||
|
|
@ -406,7 +408,7 @@ static bool above_location_current(const Location &loc)
|
|||
modify a destination to be setup for terrain following if
|
||||
TERRAIN_FOLLOW is enabled
|
||||
*/
|
||||
static void setup_terrain_target_alt(Location &loc)
|
||||
void Plane::setup_terrain_target_alt(Location &loc)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
if (g.terrain_follow) {
|
||||
|
|
@ -420,7 +422,7 @@ static void setup_terrain_target_alt(Location &loc)
|
|||
This is useful during long flights to account for barometer changes
|
||||
from the GCS, or to adjust the flying height of a long mission
|
||||
*/
|
||||
static int32_t adjusted_altitude_cm(void)
|
||||
int32_t Plane::adjusted_altitude_cm(void)
|
||||
{
|
||||
return current_loc.alt - (g.alt_offset*100);
|
||||
}
|
||||
|
|
@ -430,7 +432,7 @@ static int32_t adjusted_altitude_cm(void)
|
|||
during long flights to account for barometer changes from the GCS,
|
||||
or to adjust the flying height of a long mission
|
||||
*/
|
||||
static int32_t adjusted_relative_altitude_cm(void)
|
||||
int32_t Plane::adjusted_relative_altitude_cm(void)
|
||||
{
|
||||
return adjusted_altitude_cm() - home.alt;
|
||||
}
|
||||
|
|
@ -438,7 +440,7 @@ static int32_t adjusted_relative_altitude_cm(void)
|
|||
/*
|
||||
return the height in meters above the next_WP_loc altitude
|
||||
*/
|
||||
static float height_above_target(void)
|
||||
float Plane::height_above_target(void)
|
||||
{
|
||||
float target_alt = next_WP_loc.alt*0.01;
|
||||
if (!next_WP_loc.flags.relative_alt) {
|
||||
|
|
@ -460,7 +462,7 @@ static float height_above_target(void)
|
|||
/*
|
||||
work out target altitude adjustment from terrain lookahead
|
||||
*/
|
||||
static float lookahead_adjustment(void)
|
||||
float Plane::lookahead_adjustment(void)
|
||||
{
|
||||
#if AP_TERRAIN_AVAILABLE
|
||||
int32_t bearing_cd;
|
||||
|
|
@ -521,7 +523,7 @@ static float lookahead_adjustment(void)
|
|||
meters to correct target altitude. A positive number means we need
|
||||
to ask the speed/height controller to fly higher
|
||||
*/
|
||||
static float rangefinder_correction(void)
|
||||
float Plane::rangefinder_correction(void)
|
||||
{
|
||||
if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
|
||||
// we haven't had any rangefinder data for 5s - don't use it
|
||||
|
|
@ -544,7 +546,7 @@ static float rangefinder_correction(void)
|
|||
/*
|
||||
update the offset between rangefinder height and terrain height
|
||||
*/
|
||||
static void rangefinder_height_update(void)
|
||||
void Plane::rangefinder_height_update(void)
|
||||
{
|
||||
uint16_t distance_cm = rangefinder.distance_cm();
|
||||
float height_estimate = 0;
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
#if 0 // currently unused
|
||||
|
||||
struct DataPoint {
|
||||
|
|
@ -3,10 +3,12 @@
|
|||
* logic for dealing with the current command in the mission and home location
|
||||
*/
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
* set_next_WP - sets the target location the vehicle should fly to
|
||||
*/
|
||||
static void set_next_WP(const struct Location &loc)
|
||||
void Plane::set_next_WP(const struct Location &loc)
|
||||
{
|
||||
if (auto_state.next_wp_no_crosstrack) {
|
||||
// we should not try to cross-track for this waypoint
|
||||
|
|
@ -67,7 +69,7 @@ static void set_next_WP(const struct Location &loc)
|
|||
loiter_angle_reset();
|
||||
}
|
||||
|
||||
static void set_guided_WP(void)
|
||||
void Plane::set_guided_WP(void)
|
||||
{
|
||||
if (g.loiter_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
|
|
@ -96,7 +98,7 @@ static void set_guided_WP(void)
|
|||
|
||||
// run this at setup on the ground
|
||||
// -------------------------------
|
||||
static void init_home()
|
||||
void Plane::init_home()
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("init home"));
|
||||
|
||||
|
|
@ -118,7 +120,7 @@ static void init_home()
|
|||
this is called as long as we have 3D lock and the arming switch is
|
||||
not pushed
|
||||
*/
|
||||
static void update_home()
|
||||
void Plane::update_home()
|
||||
{
|
||||
if (home_is_set == HOME_SET_NOT_LOCKED) {
|
||||
ahrs.set_home(gps.location());
|
||||
|
|
@ -1,35 +1,11 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// forward declarations to make compiler happy
|
||||
static void do_takeoff(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_land(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_loiter_turns(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_loiter_time(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_wait_delay(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_within_distance(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_change_alt(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_change_speed(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_set_home(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd);
|
||||
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
|
||||
#if CAMERA == ENABLED
|
||||
static void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
|
||||
static void do_digicam_control(const AP_Mission::Mission_Command& cmd);
|
||||
#endif
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/********************************************************************************/
|
||||
// Command Event Handlers
|
||||
/********************************************************************************/
|
||||
|
||||
/********************************************************************************/
|
||||
// Command Event Handlers
|
||||
/********************************************************************************/
|
||||
static bool
|
||||
start_command(const AP_Mission::Mission_Command& cmd)
|
||||
bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// log when new commands start
|
||||
if (should_log(MASK_LOG_CMD)) {
|
||||
|
|
@ -213,7 +189,7 @@ operation returns true when the mission element has completed and we
|
|||
should move onto the next mission element.
|
||||
*******************************************************************************/
|
||||
|
||||
static bool verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
|
||||
bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Returns true if command complete
|
||||
{
|
||||
switch(cmd.id) {
|
||||
|
||||
|
|
@ -293,7 +269,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
|
|||
// Nav (Must) commands
|
||||
/********************************************************************************/
|
||||
|
||||
static void do_RTL(void)
|
||||
void Plane::do_RTL(void)
|
||||
{
|
||||
auto_state.next_wp_no_crosstrack = true;
|
||||
auto_state.no_crosstrack = true;
|
||||
|
|
@ -316,7 +292,7 @@ static void do_RTL(void)
|
|||
DataFlash.Log_Write_Mode(control_mode);
|
||||
}
|
||||
|
||||
static void do_takeoff(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_takeoff(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
prev_WP_loc = current_loc;
|
||||
set_next_WP(cmd.content.location);
|
||||
|
|
@ -334,18 +310,18 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
}
|
||||
|
||||
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_next_WP(cmd.content.location);
|
||||
}
|
||||
|
||||
static void do_land(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_land(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
auto_state.commanded_go_around = false;
|
||||
set_next_WP(cmd.content.location);
|
||||
}
|
||||
|
||||
static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.content.location.flags.loiter_ccw) {
|
||||
loiter.direction = -1;
|
||||
|
|
@ -354,20 +330,20 @@ static void loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
}
|
||||
|
||||
static void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_next_WP(cmd.content.location);
|
||||
loiter_set_direction_wp(cmd);
|
||||
}
|
||||
|
||||
static void do_loiter_turns(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_loiter_turns(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_next_WP(cmd.content.location);
|
||||
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
|
||||
loiter_set_direction_wp(cmd);
|
||||
}
|
||||
|
||||
static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
set_next_WP(cmd.content.location);
|
||||
// we set start_time_ms when we reach the waypoint
|
||||
|
|
@ -376,13 +352,13 @@ static void do_loiter_time(const AP_Mission::Mission_Command& cmd)
|
|||
loiter_set_direction_wp(cmd);
|
||||
}
|
||||
|
||||
static void do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_continue_and_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
next_WP_loc.alt = cmd.content.location.alt + home.alt;
|
||||
reset_offset_altitude();
|
||||
}
|
||||
|
||||
static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
//set target alt
|
||||
next_WP_loc.alt = cmd.content.location.alt;
|
||||
|
|
@ -411,7 +387,7 @@ static void do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
|
|||
/********************************************************************************/
|
||||
// Verify Nav (Must) commands
|
||||
/********************************************************************************/
|
||||
static bool verify_takeoff()
|
||||
bool Plane::verify_takeoff()
|
||||
{
|
||||
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
|
||||
const float min_gps_speed = 5;
|
||||
|
|
@ -477,7 +453,7 @@ static bool verify_takeoff()
|
|||
update navigation for normal mission waypoints. Return true when the
|
||||
waypoint is complete
|
||||
*/
|
||||
static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
bool Plane::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
steer_state.hold_course_cd = -1;
|
||||
|
||||
|
|
@ -521,28 +497,28 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool verify_loiter_unlim()
|
||||
bool Plane::verify_loiter_unlim()
|
||||
{
|
||||
update_loiter();
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_loiter_time()
|
||||
bool Plane::verify_loiter_time()
|
||||
{
|
||||
update_loiter();
|
||||
if (loiter.start_time_ms == 0) {
|
||||
if (nav_controller->reached_loiter_target()) {
|
||||
// we've reached the target, start the timer
|
||||
loiter.start_time_ms = millis();
|
||||
loiter.start_time_ms = hal.scheduler->millis();
|
||||
}
|
||||
} else if ((millis() - loiter.start_time_ms) > loiter.time_max_ms) {
|
||||
} else if ((hal.scheduler->millis() - loiter.start_time_ms) > loiter.time_max_ms) {
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_loiter_turns()
|
||||
bool Plane::verify_loiter_turns()
|
||||
{
|
||||
update_loiter();
|
||||
if (loiter.sum_cd > loiter.total_cd) {
|
||||
|
|
@ -559,7 +535,7 @@ static bool verify_loiter_turns()
|
|||
reached both the desired altitude and desired heading. The desired
|
||||
altitude only needs to be reached once.
|
||||
*/
|
||||
static bool verify_loiter_to_alt()
|
||||
bool Plane::verify_loiter_to_alt()
|
||||
{
|
||||
update_loiter();
|
||||
|
||||
|
|
@ -618,7 +594,7 @@ static bool verify_loiter_to_alt()
|
|||
return true;
|
||||
}
|
||||
|
||||
static bool verify_RTL()
|
||||
bool Plane::verify_RTL()
|
||||
{
|
||||
update_loiter();
|
||||
if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) ||
|
||||
|
|
@ -630,7 +606,7 @@ static bool verify_RTL()
|
|||
}
|
||||
}
|
||||
|
||||
static bool verify_continue_and_change_alt()
|
||||
bool Plane::verify_continue_and_change_alt()
|
||||
{
|
||||
if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
|
||||
return true;
|
||||
|
|
@ -653,16 +629,16 @@ static bool verify_continue_and_change_alt()
|
|||
// Condition (May) commands
|
||||
/********************************************************************************/
|
||||
|
||||
static void do_wait_delay(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_wait_delay(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_start = millis();
|
||||
condition_start = hal.scheduler->millis();
|
||||
condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
|
||||
}
|
||||
|
||||
/*
|
||||
process a DO_CHANGE_ALT request
|
||||
*/
|
||||
static void do_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_change_alt(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_rate = labs((int)cmd.content.location.lat); // climb rate in cm/s
|
||||
condition_value = cmd.content.location.alt; // To-Do: ensure this altitude is an absolute altitude?
|
||||
|
|
@ -676,7 +652,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
|
|||
setup_glide_slope();
|
||||
}
|
||||
|
||||
static void do_within_distance(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_within_distance(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
condition_value = cmd.content.distance.meters;
|
||||
}
|
||||
|
|
@ -685,16 +661,16 @@ static void do_within_distance(const AP_Mission::Mission_Command& cmd)
|
|||
// Verify Condition (May) commands
|
||||
/********************************************************************************/
|
||||
|
||||
static bool verify_wait_delay()
|
||||
bool Plane::verify_wait_delay()
|
||||
{
|
||||
if ((unsigned)(millis() - condition_start) > (unsigned)condition_value) {
|
||||
if ((unsigned)(hal.scheduler->millis() - condition_start) > (unsigned)condition_value) {
|
||||
condition_value = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool verify_change_alt()
|
||||
bool Plane::verify_change_alt()
|
||||
{
|
||||
if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) ||
|
||||
(condition_rate<=0 && adjusted_altitude_cm() <= condition_value)) {
|
||||
|
|
@ -707,7 +683,7 @@ static bool verify_change_alt()
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool verify_within_distance()
|
||||
bool Plane::verify_within_distance()
|
||||
{
|
||||
if (auto_state.wp_distance < max(condition_value,0)) {
|
||||
condition_value = 0;
|
||||
|
|
@ -720,7 +696,7 @@ static bool verify_within_distance()
|
|||
// Do (Now) commands
|
||||
/********************************************************************************/
|
||||
|
||||
static void do_loiter_at_location()
|
||||
void Plane::do_loiter_at_location()
|
||||
{
|
||||
if (g.loiter_radius < 0) {
|
||||
loiter.direction = -1;
|
||||
|
|
@ -730,7 +706,7 @@ static void do_loiter_at_location()
|
|||
next_WP_loc = current_loc;
|
||||
}
|
||||
|
||||
static void do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
switch (cmd.content.speed.speed_type)
|
||||
{
|
||||
|
|
@ -752,7 +728,7 @@ static void do_change_speed(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
}
|
||||
|
||||
static void do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_set_home(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
init_home();
|
||||
|
|
@ -763,7 +739,7 @@ static void do_set_home(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// do_digicam_configure Send Digicam Configure message with the camera library
|
||||
static void do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
#if CAMERA == ENABLED
|
||||
camera.configure_cmd(cmd);
|
||||
|
|
@ -771,7 +747,7 @@ static void do_digicam_configure(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// do_digicam_control Send Digicam Control message with the camera library
|
||||
static void do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
#if CAMERA == ENABLED
|
||||
camera.control_cmd(cmd);
|
||||
|
|
@ -780,7 +756,7 @@ static void do_digicam_control(const AP_Mission::Mission_Command& cmd)
|
|||
}
|
||||
|
||||
// do_take_picture - take a picture with the camera library
|
||||
static void do_take_picture()
|
||||
void Plane::do_take_picture()
|
||||
{
|
||||
#if CAMERA == ENABLED
|
||||
camera.trigger_pic(true);
|
||||
|
|
@ -789,7 +765,7 @@ static void do_take_picture()
|
|||
}
|
||||
|
||||
// log_picture - log picture taken and send feedback to GCS
|
||||
static void log_picture()
|
||||
void Plane::log_picture()
|
||||
{
|
||||
gcs_send_message(MSG_CAMERA_FEEDBACK);
|
||||
if (should_log(MASK_LOG_CAMERA)) {
|
||||
|
|
@ -799,7 +775,7 @@ static void log_picture()
|
|||
|
||||
// start_command_callback - callback function called from ap-mission when it begins a new mission command
|
||||
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
|
||||
static bool start_command_callback(const AP_Mission::Mission_Command &cmd)
|
||||
bool Plane::start_command_callback(const AP_Mission::Mission_Command &cmd)
|
||||
{
|
||||
if (control_mode == AUTO) {
|
||||
return start_command(cmd);
|
||||
|
|
@ -809,7 +785,7 @@ static bool start_command_callback(const AP_Mission::Mission_Command &cmd)
|
|||
|
||||
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
|
||||
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
|
||||
static bool verify_command_callback(const AP_Mission::Mission_Command& cmd)
|
||||
bool Plane::verify_command_callback(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (control_mode == AUTO) {
|
||||
return verify_command(cmd);
|
||||
|
|
@ -819,7 +795,7 @@ static bool verify_command_callback(const AP_Mission::Mission_Command& cmd)
|
|||
|
||||
// exit_mission_callback - callback function called from ap-mission when the mission has completed
|
||||
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
|
||||
static void exit_mission_callback()
|
||||
void Plane::exit_mission_callback()
|
||||
{
|
||||
if (control_mode == AUTO) {
|
||||
gcs_send_text_fmt(PSTR("Returning to Home"));
|
||||
|
|
@ -1,11 +1,10 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
// forward declaration of verify_command to keep compiler happy
|
||||
static bool verify_command(const AP_Mission::Mission_Command& cmd);
|
||||
#include "Plane.h"
|
||||
|
||||
// called by update navigation at 10Hz
|
||||
// --------------------
|
||||
static void update_commands(void)
|
||||
void Plane::update_commands(void)
|
||||
{
|
||||
if(control_mode == AUTO) {
|
||||
if (home_is_set != HOME_UNSET) {
|
||||
|
|
@ -1,12 +0,0 @@
|
|||
|
||||
#ifndef __COMPAT_H__
|
||||
#define __COMPAT_H__
|
||||
|
||||
#define HIGH 1
|
||||
#define LOW 0
|
||||
|
||||
/* Forward declarations to avoid broken auto-prototyper (coughs on '::'?) */
|
||||
static void run_cli(AP_HAL::UARTDriver *port);
|
||||
|
||||
#endif // __COMPAT_H__
|
||||
|
||||
|
|
@ -1,16 +0,0 @@
|
|||
|
||||
static void mavlink_delay(uint32_t ms)
|
||||
{
|
||||
hal.scheduler->delay(ms);
|
||||
}
|
||||
|
||||
static uint32_t millis()
|
||||
{
|
||||
return hal.scheduler->millis();
|
||||
}
|
||||
|
||||
static uint32_t micros()
|
||||
{
|
||||
return hal.scheduler->micros();
|
||||
}
|
||||
|
||||
|
|
@ -1,7 +1,8 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
static void read_control_switch()
|
||||
void Plane::read_control_switch()
|
||||
{
|
||||
static bool switch_debouncer;
|
||||
uint8_t switchPosition = readSwitch();
|
||||
|
|
@ -101,7 +102,7 @@ static void read_control_switch()
|
|||
#endif // CONFIG_HAL_BOARD
|
||||
}
|
||||
|
||||
static uint8_t readSwitch(void)
|
||||
uint8_t Plane::readSwitch(void)
|
||||
{
|
||||
uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
|
||||
if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition
|
||||
|
|
@ -113,7 +114,7 @@ static uint8_t readSwitch(void)
|
|||
return 0;
|
||||
}
|
||||
|
||||
static void reset_control_switch()
|
||||
void Plane::reset_control_switch()
|
||||
{
|
||||
oldSwitchPosition = 254;
|
||||
read_control_switch();
|
||||
|
|
@ -122,7 +123,7 @@ static void reset_control_switch()
|
|||
/*
|
||||
called when entering autotune
|
||||
*/
|
||||
static void autotune_start(void)
|
||||
void Plane::autotune_start(void)
|
||||
{
|
||||
rollController.autotune_start();
|
||||
pitchController.autotune_start();
|
||||
|
|
@ -131,7 +132,7 @@ static void autotune_start(void)
|
|||
/*
|
||||
called when exiting autotune
|
||||
*/
|
||||
static void autotune_restore(void)
|
||||
void Plane::autotune_restore(void)
|
||||
{
|
||||
rollController.autotune_restore();
|
||||
pitchController.autotune_restore();
|
||||
|
|
@ -140,7 +141,7 @@ static void autotune_restore(void)
|
|||
/*
|
||||
are we flying inverted?
|
||||
*/
|
||||
static bool fly_inverted(void)
|
||||
bool Plane::fly_inverted(void)
|
||||
{
|
||||
if (g.inverted_flight_ch != 0 && inverted_flight) {
|
||||
// controlled with INVERTED_FLIGHT_CH
|
||||
|
|
@ -1,11 +1,12 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
static void failsafe_short_on_event(enum failsafe_state fstype)
|
||||
void Plane::failsafe_short_on_event(enum failsafe_state fstype)
|
||||
{
|
||||
// This is how to handle a short loss of control signal failsafe.
|
||||
failsafe.state = fstype;
|
||||
failsafe.ch3_timer_ms = millis();
|
||||
failsafe.ch3_timer_ms = hal.scheduler->millis();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
|
||||
switch(control_mode)
|
||||
{
|
||||
|
|
@ -48,7 +49,7 @@ static void failsafe_short_on_event(enum failsafe_state fstype)
|
|||
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
||||
}
|
||||
|
||||
static void failsafe_long_on_event(enum failsafe_state fstype)
|
||||
void Plane::failsafe_long_on_event(enum failsafe_state fstype)
|
||||
{
|
||||
// This is how to handle a long loss of control signal failsafe.
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
|
||||
|
|
@ -93,7 +94,7 @@ static void failsafe_long_on_event(enum failsafe_state fstype)
|
|||
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
||||
}
|
||||
|
||||
static void failsafe_short_off_event()
|
||||
void Plane::failsafe_short_off_event()
|
||||
{
|
||||
// We're back in radio contact
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
|
||||
|
|
@ -107,7 +108,7 @@ static void failsafe_short_off_event()
|
|||
}
|
||||
}
|
||||
|
||||
void low_battery_event(void)
|
||||
void Plane::low_battery_event(void)
|
||||
{
|
||||
if (failsafe.low_battery) {
|
||||
return;
|
||||
|
|
@ -120,7 +121,7 @@ void low_battery_event(void)
|
|||
AP_Notify::flags.failsafe_battery = true;
|
||||
}
|
||||
|
||||
static void update_events(void)
|
||||
void Plane::update_events(void)
|
||||
{
|
||||
ServoRelayEvents.update_events();
|
||||
}
|
||||
|
|
@ -1,4 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
* failsafe support
|
||||
* Andrew Tridgell, December 2011
|
||||
|
|
@ -13,7 +16,7 @@
|
|||
* this failsafe_check function is called from the core timer interrupt
|
||||
* at 1kHz.
|
||||
*/
|
||||
void failsafe_check(void)
|
||||
void Plane::failsafe_check(void)
|
||||
{
|
||||
static uint16_t last_mainLoop_count;
|
||||
static uint32_t last_timestamp;
|
||||
|
|
@ -4,6 +4,8 @@
|
|||
* Andrew Tridgell, December 2011
|
||||
*/
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
#if GEOFENCE_ENABLED == ENABLED
|
||||
|
||||
#define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle)
|
||||
|
|
@ -45,7 +47,7 @@ static const StorageAccess fence_storage(StorageManager::StorageFence);
|
|||
/*
|
||||
maximum number of fencepoints
|
||||
*/
|
||||
static uint8_t max_fencepoints(void)
|
||||
uint8_t Plane::max_fencepoints(void)
|
||||
{
|
||||
return min(255, fence_storage.size() / sizeof(Vector2l));
|
||||
}
|
||||
|
|
@ -53,7 +55,7 @@ static uint8_t max_fencepoints(void)
|
|||
/*
|
||||
* fence boundaries fetch/store
|
||||
*/
|
||||
static Vector2l get_fence_point_with_index(unsigned i)
|
||||
Vector2l Plane::get_fence_point_with_index(unsigned i)
|
||||
{
|
||||
Vector2l ret;
|
||||
|
||||
|
|
@ -69,7 +71,7 @@ static Vector2l get_fence_point_with_index(unsigned i)
|
|||
}
|
||||
|
||||
// save a fence point
|
||||
static void set_fence_point_with_index(Vector2l &point, unsigned i)
|
||||
void Plane::set_fence_point_with_index(Vector2l &point, unsigned i)
|
||||
{
|
||||
if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) {
|
||||
// not allowed
|
||||
|
|
@ -87,7 +89,7 @@ static void set_fence_point_with_index(Vector2l &point, unsigned i)
|
|||
/*
|
||||
* allocate and fill the geofence state structure
|
||||
*/
|
||||
static void geofence_load(void)
|
||||
void Plane::geofence_load(void)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
|
|
@ -147,7 +149,7 @@ failed:
|
|||
* return true if a geo-fence has been uploaded and
|
||||
* FENCE_ACTION is 1 (not necessarily enabled)
|
||||
*/
|
||||
static bool geofence_present(void)
|
||||
bool Plane::geofence_present(void)
|
||||
{
|
||||
//require at least a return point and a triangle
|
||||
//to define a geofence area:
|
||||
|
|
@ -160,7 +162,7 @@ static bool geofence_present(void)
|
|||
/*
|
||||
check FENCE_CHANNEL and update the is_pwm_enabled state
|
||||
*/
|
||||
static void geofence_update_pwm_enabled_state()
|
||||
void Plane::geofence_update_pwm_enabled_state()
|
||||
{
|
||||
bool is_pwm_enabled;
|
||||
if (g.fence_channel == 0) {
|
||||
|
|
@ -188,7 +190,7 @@ static void geofence_update_pwm_enabled_state()
|
|||
}
|
||||
|
||||
//return true on success, false on failure
|
||||
static bool geofence_set_enabled(bool enable, GeofenceEnableReason r)
|
||||
bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r)
|
||||
{
|
||||
if (geofence_state == NULL && enable) {
|
||||
geofence_load();
|
||||
|
|
@ -210,7 +212,7 @@ static bool geofence_set_enabled(bool enable, GeofenceEnableReason r)
|
|||
/*
|
||||
* return true if geo-fencing is enabled
|
||||
*/
|
||||
static bool geofence_enabled(void)
|
||||
bool Plane::geofence_enabled(void)
|
||||
{
|
||||
geofence_update_pwm_enabled_state();
|
||||
|
||||
|
|
@ -234,7 +236,7 @@ static bool geofence_enabled(void)
|
|||
* Set floor state IF the fence is present.
|
||||
* Return false on failure to set floor state.
|
||||
*/
|
||||
static bool geofence_set_floor_enabled(bool floor_enable) {
|
||||
bool Plane::geofence_set_floor_enabled(bool floor_enable) {
|
||||
if (geofence_state == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
|
@ -246,7 +248,7 @@ static bool geofence_set_floor_enabled(bool floor_enable) {
|
|||
/*
|
||||
* return true if we have breached the geo-fence minimum altiude
|
||||
*/
|
||||
static bool geofence_check_minalt(void)
|
||||
bool Plane::geofence_check_minalt(void)
|
||||
{
|
||||
if (g.fence_maxalt <= g.fence_minalt) {
|
||||
return false;
|
||||
|
|
@ -260,7 +262,7 @@ static bool geofence_check_minalt(void)
|
|||
/*
|
||||
* return true if we have breached the geo-fence maximum altiude
|
||||
*/
|
||||
static bool geofence_check_maxalt(void)
|
||||
bool Plane::geofence_check_maxalt(void)
|
||||
{
|
||||
if (g.fence_maxalt <= g.fence_minalt) {
|
||||
return false;
|
||||
|
|
@ -275,7 +277,7 @@ static bool geofence_check_maxalt(void)
|
|||
/*
|
||||
* check if we have breached the geo-fence
|
||||
*/
|
||||
static void geofence_check(bool altitude_check_only)
|
||||
void Plane::geofence_check(bool altitude_check_only)
|
||||
{
|
||||
if (!geofence_enabled()) {
|
||||
// switch back to the chosen control mode if still in
|
||||
|
|
@ -354,7 +356,7 @@ static void geofence_check(bool altitude_check_only)
|
|||
// we are outside, and have not previously triggered.
|
||||
geofence_state->fence_triggered = true;
|
||||
geofence_state->breach_count++;
|
||||
geofence_state->breach_time = millis();
|
||||
geofence_state->breach_time = hal.scheduler->millis();
|
||||
geofence_state->breach_type = breach_type;
|
||||
|
||||
#if FENCE_TRIGGERED_PIN > 0
|
||||
|
|
@ -420,7 +422,7 @@ static void geofence_check(bool altitude_check_only)
|
|||
* disabled. Otherwise the aircraft may not be able to recover from
|
||||
* a breach of the geo-fence
|
||||
*/
|
||||
static bool geofence_stickmixing(void) {
|
||||
bool Plane::geofence_stickmixing(void) {
|
||||
if (geofence_enabled() &&
|
||||
geofence_state != NULL &&
|
||||
geofence_state->fence_triggered &&
|
||||
|
|
@ -435,7 +437,7 @@ static bool geofence_stickmixing(void) {
|
|||
/*
|
||||
*
|
||||
*/
|
||||
static void geofence_send_status(mavlink_channel_t chan)
|
||||
void Plane::geofence_send_status(mavlink_channel_t chan)
|
||||
{
|
||||
if (geofence_enabled() && geofence_state != NULL) {
|
||||
mavlink_msg_fence_status_send(chan,
|
||||
|
|
@ -449,7 +451,7 @@ static void geofence_send_status(mavlink_channel_t chan)
|
|||
/*
|
||||
return true if geofence has been breached
|
||||
*/
|
||||
static bool geofence_breached(void)
|
||||
bool Plane::geofence_breached(void)
|
||||
{
|
||||
return geofence_state ? geofence_state->fence_triggered : false;
|
||||
}
|
||||
|
|
@ -457,24 +459,24 @@ static bool geofence_breached(void)
|
|||
|
||||
#else // GEOFENCE_ENABLED
|
||||
|
||||
static void geofence_check(bool altitude_check_only) {
|
||||
void Plane::geofence_check(bool altitude_check_only) {
|
||||
}
|
||||
static bool geofence_stickmixing(void) {
|
||||
bool Plane::geofence_stickmixing(void) {
|
||||
return true;
|
||||
}
|
||||
static bool geofence_enabled(void) {
|
||||
bool Plane::geofence_enabled(void) {
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool geofence_present(void) {
|
||||
bool Plane::geofence_present(void) {
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) {
|
||||
bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) {
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool geofence_set_floor_enabled(bool floor_enable) {
|
||||
bool Plane::geofence_set_floor_enabled(bool floor_enable) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
landing logic
|
||||
*/
|
||||
|
|
@ -8,7 +10,7 @@
|
|||
update navigation for landing. Called when on landing approach or
|
||||
final flare
|
||||
*/
|
||||
static bool verify_land()
|
||||
bool Plane::verify_land()
|
||||
{
|
||||
// we don't 'verify' landing in the sense that it never completes,
|
||||
// so we don't verify command completion. Instead we use this to
|
||||
|
|
@ -102,7 +104,7 @@ static bool verify_land()
|
|||
/*
|
||||
If land_DisarmDelay is enabled (non-zero), check for a landing then auto-disarm after time expires
|
||||
*/
|
||||
static void disarm_if_autoland_complete()
|
||||
void Plane::disarm_if_autoland_complete()
|
||||
{
|
||||
if (g.land_disarm_delay > 0 &&
|
||||
auto_state.land_complete &&
|
||||
|
|
@ -128,7 +130,7 @@ static void disarm_if_autoland_complete()
|
|||
itself as that leads to discontinuities close to the landing point,
|
||||
which can lead to erratic pitch control
|
||||
*/
|
||||
static void setup_landing_glide_slope(void)
|
||||
void Plane::setup_landing_glide_slope(void)
|
||||
{
|
||||
Location loc = next_WP_loc;
|
||||
|
||||
|
|
@ -209,7 +211,7 @@ static void setup_landing_glide_slope(void)
|
|||
switch to that mission item. Returns false if no DO_LAND_START
|
||||
available.
|
||||
*/
|
||||
static bool jump_to_landing_sequence(void)
|
||||
bool Plane::jump_to_landing_sequence(void)
|
||||
{
|
||||
uint16_t land_idx = mission.get_landing_sequence_start();
|
||||
if (land_idx != 0) {
|
||||
|
|
@ -233,7 +235,7 @@ static bool jump_to_landing_sequence(void)
|
|||
/*
|
||||
the height above field elevation that we pass to TECS
|
||||
*/
|
||||
static float tecs_hgt_afe(void)
|
||||
float Plane::tecs_hgt_afe(void)
|
||||
{
|
||||
/*
|
||||
pass the height above field elevation as the height above
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
LIBRARIES = AP_HAL
|
||||
LIBRARIES += AP_Common
|
||||
LIBRARIES += AP_Progmem
|
||||
LIBRARIES += AP_Menu
|
||||
LIBRARIES += AP_Param
|
||||
LIBRARIES += StorageManager
|
||||
LIBRARIES += AP_GPS
|
||||
LIBRARIES += AP_Baro
|
||||
LIBRARIES += AP_Compass
|
||||
LIBRARIES += AP_Math
|
||||
LIBRARIES += AP_ADC
|
||||
LIBRARIES += AP_ADC_AnalogSource
|
||||
LIBRARIES += AP_InertialSensor
|
||||
LIBRARIES += AP_AHRS
|
||||
LIBRARIES += RC_Channel
|
||||
LIBRARIES += AP_RangeFinder
|
||||
LIBRARIES += Filter
|
||||
LIBRARIES += AP_Buffer
|
||||
LIBRARIES += AP_Relay
|
||||
LIBRARIES += AP_Camera
|
||||
LIBRARIES += AP_Airspeed
|
||||
LIBRARIES += AP_Terrain
|
||||
LIBRARIES += APM_OBC
|
||||
LIBRARIES += APM_Control
|
||||
LIBRARIES += AP_AutoTune
|
||||
LIBRARIES += GCS
|
||||
LIBRARIES += GCS_MAVLink
|
||||
LIBRARIES += AP_SerialManager
|
||||
LIBRARIES += AP_Mount
|
||||
LIBRARIES += AP_Declination
|
||||
LIBRARIES += DataFlash
|
||||
LIBRARIES += SITL
|
||||
LIBRARIES += AP_Scheduler
|
||||
LIBRARIES += AP_Navigation
|
||||
LIBRARIES += AP_L1_Control
|
||||
LIBRARIES += AP_RCMapper
|
||||
LIBRARIES += AP_Vehicle
|
||||
LIBRARIES += AP_SpdHgtControl
|
||||
LIBRARIES += AP_TECS
|
||||
LIBRARIES += AP_NavEKF
|
||||
LIBRARIES += AP_Mission
|
||||
LIBRARIES += AP_Notify
|
||||
LIBRARIES += AP_BattMonitor
|
||||
LIBRARIES += AP_Arming
|
||||
LIBRARIES += AP_BoardConfig
|
||||
LIBRARIES += AP_Frsky_Telem
|
||||
LIBRARIES += AP_ServoRelayEvents
|
||||
LIBRARIES += AP_Rally
|
||||
LIBRARIES += AP_OpticalFlow
|
||||
LIBRARIES += AP_HAL_AVR
|
||||
LIBRARIES += AP_HAL_SITL
|
||||
LIBRARIES += AP_HAL_PX4
|
||||
LIBRARIES += AP_HAL_FLYMAPLE
|
||||
LIBRARIES += AP_HAL_Linux
|
||||
LIBRARIES += AP_HAL_Empty
|
||||
LIBRARIES += AP_HAL_VRBRAIN
|
||||
|
|
@ -1,8 +1,9 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
// set the nav_controller pointer to the right controller
|
||||
static void set_nav_controller(void)
|
||||
void Plane::set_nav_controller(void)
|
||||
{
|
||||
switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
|
||||
case AP_Navigation::CONTROLLER_L1:
|
||||
|
|
@ -14,7 +15,7 @@ static void set_nav_controller(void)
|
|||
/*
|
||||
reset the total loiter angle
|
||||
*/
|
||||
static void loiter_angle_reset(void)
|
||||
void Plane::loiter_angle_reset(void)
|
||||
{
|
||||
loiter.sum_cd = 0;
|
||||
loiter.total_cd = 0;
|
||||
|
|
@ -24,7 +25,7 @@ static void loiter_angle_reset(void)
|
|||
update the total angle we have covered in a loiter. Used to support
|
||||
commands to do N circles of loiter
|
||||
*/
|
||||
static void loiter_angle_update(void)
|
||||
void Plane::loiter_angle_update(void)
|
||||
{
|
||||
int32_t target_bearing_cd = nav_controller->target_bearing_cd();
|
||||
int32_t loiter_delta_cd;
|
||||
|
|
@ -43,7 +44,7 @@ static void loiter_angle_update(void)
|
|||
//****************************************************************
|
||||
// Function that will calculate the desired direction to fly and distance
|
||||
//****************************************************************
|
||||
static void navigate()
|
||||
void Plane::navigate()
|
||||
{
|
||||
// allow change of nav controller mid-flight
|
||||
set_nav_controller();
|
||||
|
|
@ -72,7 +73,7 @@ static void navigate()
|
|||
update_navigation();
|
||||
}
|
||||
|
||||
static void calc_airspeed_errors()
|
||||
void Plane::calc_airspeed_errors()
|
||||
{
|
||||
float aspeed_cm = airspeed.get_airspeed_cm();
|
||||
|
||||
|
|
@ -111,7 +112,7 @@ static void calc_airspeed_errors()
|
|||
airspeed_error_cm = SpdHgt_Controller->get_target_airspeed()*100 - aspeed_cm;
|
||||
}
|
||||
|
||||
static void calc_gndspeed_undershoot()
|
||||
void Plane::calc_gndspeed_undershoot()
|
||||
{
|
||||
// Use the component of ground speed in the forward direction
|
||||
// This prevents flyaway if wind takes plane backwards
|
||||
|
|
@ -125,7 +126,7 @@ static void calc_gndspeed_undershoot()
|
|||
}
|
||||
}
|
||||
|
||||
static void update_loiter()
|
||||
void Plane::update_loiter()
|
||||
{
|
||||
nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction);
|
||||
}
|
||||
|
|
@ -134,7 +135,7 @@ static void update_loiter()
|
|||
handle CRUISE mode, locking heading to GPS course when we have
|
||||
sufficient ground speed, and no aileron or rudder input
|
||||
*/
|
||||
static void update_cruise()
|
||||
void Plane::update_cruise()
|
||||
{
|
||||
if (!cruise_state.locked_heading &&
|
||||
channel_roll->control_in == 0 &&
|
||||
|
|
@ -170,7 +171,7 @@ static void update_cruise()
|
|||
In this mode the elevator is used to change target altitude. The
|
||||
throttle is used to change target airspeed or throttle
|
||||
*/
|
||||
static void update_fbwb_speed_height(void)
|
||||
void Plane::update_fbwb_speed_height(void)
|
||||
{
|
||||
static float last_elevator_input;
|
||||
float elevator_input;
|
||||
|
|
@ -202,7 +203,7 @@ static void update_fbwb_speed_height(void)
|
|||
/*
|
||||
calculate the turn angle for the next leg of the mission
|
||||
*/
|
||||
static void setup_turn_angle(void)
|
||||
void Plane::setup_turn_angle(void)
|
||||
{
|
||||
int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
|
||||
if (next_ground_course_cd == -1) {
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*
|
||||
handle creation of PX4 mixer file, for failover to direct RC control
|
||||
on failure of FMU
|
||||
|
|
@ -23,7 +25,7 @@
|
|||
/*
|
||||
create a mixer file given key fixed wing parameters
|
||||
*/
|
||||
static bool create_mixer_file(const char *filename)
|
||||
bool Plane::create_mixer_file(const char *filename)
|
||||
{
|
||||
int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
|
||||
if (mix_fd == -1) {
|
||||
|
|
@ -185,7 +187,7 @@ static bool create_mixer_file(const char *filename)
|
|||
/*
|
||||
setup mixer on PX4 so that if FMU dies the pilot gets manual control
|
||||
*/
|
||||
static bool setup_failsafe_mixing(void)
|
||||
bool Plane::setup_failsafe_mixing(void)
|
||||
{
|
||||
// we create MIXER.MIX regardless of whether we will be using it,
|
||||
// as it gives a template for the user to modify to create their
|
||||
|
|
@ -1,12 +1,14 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
//Function that will read the radio data, limit servos and trigger a failsafe
|
||||
// ----------------------------------------------------------------------------
|
||||
|
||||
/*
|
||||
allow for runtime change of control channel ordering
|
||||
*/
|
||||
static void set_control_channels(void)
|
||||
void Plane::set_control_channels(void)
|
||||
{
|
||||
if (g.rudder_only) {
|
||||
// in rudder only mode the roll and rudder channels are the
|
||||
|
|
@ -37,7 +39,7 @@ static void set_control_channels(void)
|
|||
/*
|
||||
initialise RC input channels
|
||||
*/
|
||||
static void init_rc_in()
|
||||
void Plane::init_rc_in()
|
||||
{
|
||||
// set rc dead zones
|
||||
channel_roll->set_default_dead_zone(30);
|
||||
|
|
@ -51,7 +53,7 @@ static void init_rc_in()
|
|||
/*
|
||||
initialise RC output channels
|
||||
*/
|
||||
static void init_rc_out()
|
||||
void Plane::init_rc_out()
|
||||
{
|
||||
channel_roll->enable_out();
|
||||
channel_pitch->enable_out();
|
||||
|
|
@ -75,7 +77,7 @@ static void init_rc_out()
|
|||
}
|
||||
|
||||
// check for pilot input on rudder stick for arming
|
||||
static void rudder_arm_check()
|
||||
void Plane::rudder_arm_check()
|
||||
{
|
||||
//TODO: ensure rudder arming disallowed during radio calibration
|
||||
|
||||
|
|
@ -108,7 +110,7 @@ static void rudder_arm_check()
|
|||
|
||||
// full right rudder starts arming counter
|
||||
if (channel_rudder->control_in > 4000) {
|
||||
uint32_t now = millis();
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
|
||||
if (rudder_arm_timer == 0 ||
|
||||
now - rudder_arm_timer < 3000) {
|
||||
|
|
@ -124,7 +126,7 @@ static void rudder_arm_check()
|
|||
}
|
||||
}
|
||||
|
||||
static void read_radio()
|
||||
void Plane::read_radio()
|
||||
{
|
||||
if (!hal.rcin->new_input()) {
|
||||
control_failsafe(channel_throttle->radio_in);
|
||||
|
|
@ -186,7 +188,7 @@ static void read_radio()
|
|||
}
|
||||
}
|
||||
|
||||
static void control_failsafe(uint16_t pwm)
|
||||
void Plane::control_failsafe(uint16_t pwm)
|
||||
{
|
||||
if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
|
||||
// we do not have valid RC input. Set all primary channel
|
||||
|
|
@ -238,7 +240,7 @@ static void control_failsafe(uint16_t pwm)
|
|||
}
|
||||
}
|
||||
|
||||
static void trim_control_surfaces()
|
||||
void Plane::trim_control_surfaces()
|
||||
{
|
||||
read_radio();
|
||||
int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5;
|
||||
|
|
@ -292,7 +294,7 @@ static void trim_control_surfaces()
|
|||
channel_rudder->save_eeprom();
|
||||
}
|
||||
|
||||
static void trim_radio()
|
||||
void Plane::trim_radio()
|
||||
{
|
||||
for (uint8_t y = 0; y < 30; y++) {
|
||||
read_radio();
|
||||
|
|
@ -305,7 +307,7 @@ static void trim_radio()
|
|||
return true if throttle level is below throttle failsafe threshold
|
||||
or RC input is invalid
|
||||
*/
|
||||
static bool rc_failsafe_active(void)
|
||||
bool Plane::rc_failsafe_active(void)
|
||||
{
|
||||
if (!g.throttle_fs_enabled) {
|
||||
return false;
|
||||
|
|
@ -1,6 +1,8 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
static void init_barometer(void)
|
||||
#include "Plane.h"
|
||||
|
||||
void Plane::init_barometer(void)
|
||||
{
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
|
||||
barometer.calibrate();
|
||||
|
|
@ -8,7 +10,7 @@ static void init_barometer(void)
|
|||
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
|
||||
}
|
||||
|
||||
static void init_rangefinder(void)
|
||||
void Plane::init_rangefinder(void)
|
||||
{
|
||||
rangefinder.init();
|
||||
}
|
||||
|
|
@ -16,7 +18,7 @@ static void init_rangefinder(void)
|
|||
/*
|
||||
read the rangefinder and update height estimate
|
||||
*/
|
||||
static void read_rangefinder(void)
|
||||
void Plane::read_rangefinder(void)
|
||||
{
|
||||
rangefinder.update();
|
||||
|
||||
|
|
@ -29,7 +31,7 @@ static void read_rangefinder(void)
|
|||
/*
|
||||
ask airspeed sensor for a new value
|
||||
*/
|
||||
static void read_airspeed(void)
|
||||
void Plane::read_airspeed(void)
|
||||
{
|
||||
if (airspeed.enabled()) {
|
||||
airspeed.read();
|
||||
|
|
@ -53,7 +55,7 @@ static void read_airspeed(void)
|
|||
}
|
||||
}
|
||||
|
||||
static void zero_airspeed(bool in_startup)
|
||||
void Plane::zero_airspeed(bool in_startup)
|
||||
{
|
||||
airspeed.calibrate(in_startup);
|
||||
read_airspeed();
|
||||
|
|
@ -64,7 +66,7 @@ static void zero_airspeed(bool in_startup)
|
|||
|
||||
// read_battery - reads battery voltage and current and invokes failsafe
|
||||
// should be called at 10hz
|
||||
static void read_battery(void)
|
||||
void Plane::read_battery(void)
|
||||
{
|
||||
battery.read();
|
||||
compass.set_current(battery.current_amps());
|
||||
|
|
@ -77,7 +79,7 @@ static void read_battery(void)
|
|||
|
||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||
// RC_CHANNELS_SCALED message
|
||||
void read_receiver_rssi(void)
|
||||
void Plane::read_receiver_rssi(void)
|
||||
{
|
||||
// avoid divide by zero
|
||||
if (g.rssi_range <= 0) {
|
||||
|
|
@ -0,0 +1,81 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
||||
// Command/function table for the setup menu
|
||||
static const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
// command function called
|
||||
// ======= ===============
|
||||
{"reset", MENU_FUNC(setup_factory)},
|
||||
{"erase", MENU_FUNC(setup_erase)}
|
||||
};
|
||||
|
||||
// Create the setup menu object.
|
||||
MENU(setup_menu, "setup", setup_menu_commands);
|
||||
|
||||
// Called from the top-level menu to run the setup menu.
|
||||
int8_t Plane::setup_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
// Give the user some guidance
|
||||
cliSerial->printf_P(PSTR("Setup Mode\n"
|
||||
"\n"
|
||||
"IMPORTANT: if you have not previously set this system up, use the\n"
|
||||
"'reset' command to initialize the EEPROM to sensible default values\n"
|
||||
"and then the 'radio' command to configure for your radio.\n"
|
||||
"\n"));
|
||||
|
||||
// Run the setup menu. When the menu exits, we will return to the main menu.
|
||||
setup_menu.run();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
||||
// Called by the setup menu 'factoryreset' command.
|
||||
int8_t Plane::setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int c;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
|
||||
|
||||
do {
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
AP_Param::erase_all();
|
||||
cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset board to continue"));
|
||||
|
||||
for (;; ) {
|
||||
}
|
||||
// note, cannot actually return here
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
int8_t Plane::setup_erase(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int c;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
|
||||
|
||||
do {
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
zero_eeprom();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void Plane::zero_eeprom(void)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
|
||||
StorageManager::erase();
|
||||
cliSerial->printf_P(PSTR("done\n"));
|
||||
}
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
|
|
@ -1,441 +0,0 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
||||
// Functions called from the setup menu
|
||||
static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_set (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
|
||||
// Command/function table for the setup menu
|
||||
static const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
// command function called
|
||||
// ======= ===============
|
||||
{"reset", setup_factory},
|
||||
{"radio", setup_radio},
|
||||
{"accel", setup_accel_scale},
|
||||
{"compass", setup_compass},
|
||||
{"show", setup_show},
|
||||
{"set", setup_set},
|
||||
{"erase", setup_erase},
|
||||
};
|
||||
|
||||
// Create the setup menu object.
|
||||
MENU(setup_menu, "setup", setup_menu_commands);
|
||||
|
||||
// Called from the top-level menu to run the setup menu.
|
||||
static int8_t
|
||||
setup_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
// Give the user some guidance
|
||||
cliSerial->printf_P(PSTR("Setup Mode\n"
|
||||
"\n"
|
||||
"IMPORTANT: if you have not previously set this system up, use the\n"
|
||||
"'reset' command to initialize the EEPROM to sensible default values\n"
|
||||
"and then the 'radio' command to configure for your radio.\n"
|
||||
"\n"));
|
||||
|
||||
// Run the setup menu. When the menu exits, we will return to the main menu.
|
||||
setup_menu.run();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Print the current configuration.
|
||||
// Called by the setup menu 'show' command.
|
||||
static int8_t
|
||||
setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
AP_Param *param;
|
||||
ap_var_type type;
|
||||
|
||||
//If a parameter name is given as an argument to show, print only that parameter
|
||||
if(argc>=2)
|
||||
{
|
||||
|
||||
param=AP_Param::find(argv[1].str, &type);
|
||||
|
||||
if(!param)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Parameter not found: '%s'\n"), argv[1]);
|
||||
return 0;
|
||||
}
|
||||
AP_Param::show(param, argv[1].str, type, cliSerial);
|
||||
return 0;
|
||||
}
|
||||
|
||||
AP_Param::show_all(cliSerial);
|
||||
|
||||
return(0);
|
||||
}
|
||||
|
||||
//Set a parameter to a specified value. It will cast the value to the current type of the
|
||||
//parameter and make sure it fits in case of INT8 and INT16
|
||||
static int8_t setup_set(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int8_t value_int8;
|
||||
int16_t value_int16;
|
||||
|
||||
AP_Param *param;
|
||||
enum ap_var_type p_type;
|
||||
|
||||
if(argc!=3)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Invalid command. Usage: set <name> <value>\n"));
|
||||
return 0;
|
||||
}
|
||||
|
||||
param = AP_Param::find(argv[1].str, &p_type);
|
||||
if(!param)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Param not found: %s\n"), argv[1].str);
|
||||
return 0;
|
||||
}
|
||||
|
||||
switch(p_type)
|
||||
{
|
||||
case AP_PARAM_INT8:
|
||||
value_int8 = (int8_t)(argv[2].i);
|
||||
if(argv[2].i!=value_int8)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Value out of range for type INT8\n"));
|
||||
return 0;
|
||||
}
|
||||
((AP_Int8*)param)->set_and_save(value_int8);
|
||||
break;
|
||||
case AP_PARAM_INT16:
|
||||
value_int16 = (int16_t)(argv[2].i);
|
||||
if(argv[2].i!=value_int16)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Value out of range for type INT16\n"));
|
||||
return 0;
|
||||
}
|
||||
((AP_Int16*)param)->set_and_save(value_int16);
|
||||
break;
|
||||
|
||||
//int32 and float don't need bounds checking, just use the value provoded by Menu::arg
|
||||
case AP_PARAM_INT32:
|
||||
((AP_Int32*)param)->set_and_save(argv[2].i);
|
||||
break;
|
||||
case AP_PARAM_FLOAT:
|
||||
((AP_Float*)param)->set_and_save(argv[2].f);
|
||||
break;
|
||||
default:
|
||||
cliSerial->printf_P(PSTR("Cannot set parameter of type %d.\n"), p_type);
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Initialise the EEPROM to 'factory' settings (mostly defined in APM_Config.h or via defaults).
|
||||
// Called by the setup menu 'factoryreset' command.
|
||||
static int8_t
|
||||
setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int c;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to perform factory reset, any other key to abort: "));
|
||||
|
||||
do {
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
AP_Param::erase_all();
|
||||
cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset board to continue"));
|
||||
|
||||
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
|
||||
|
||||
for (;; ) {
|
||||
}
|
||||
// note, cannot actually return here
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
// Perform radio setup.
|
||||
// Called by the setup menu 'radio' command.
|
||||
static int8_t
|
||||
setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\n\nRadio Setup:\n"));
|
||||
uint8_t i;
|
||||
|
||||
for(i = 0; i < 100; i++) {
|
||||
hal.scheduler->delay(20);
|
||||
read_radio();
|
||||
}
|
||||
|
||||
|
||||
if(channel_roll->radio_in < 500) {
|
||||
while(1) {
|
||||
cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
hal.scheduler->delay(1000);
|
||||
// stop here
|
||||
}
|
||||
}
|
||||
|
||||
channel_roll->radio_min = channel_roll->radio_in;
|
||||
channel_pitch->radio_min = channel_pitch->radio_in;
|
||||
channel_throttle->radio_min = channel_throttle->radio_in;
|
||||
channel_rudder->radio_min = channel_rudder->radio_in;
|
||||
g.rc_5.radio_min = g.rc_5.radio_in;
|
||||
g.rc_6.radio_min = g.rc_6.radio_in;
|
||||
g.rc_7.radio_min = g.rc_7.radio_in;
|
||||
g.rc_8.radio_min = g.rc_8.radio_in;
|
||||
|
||||
channel_roll->radio_max = channel_roll->radio_in;
|
||||
channel_pitch->radio_max = channel_pitch->radio_in;
|
||||
channel_throttle->radio_max = channel_throttle->radio_in;
|
||||
channel_rudder->radio_max = channel_rudder->radio_in;
|
||||
g.rc_5.radio_max = g.rc_5.radio_in;
|
||||
g.rc_6.radio_max = g.rc_6.radio_in;
|
||||
g.rc_7.radio_max = g.rc_7.radio_in;
|
||||
g.rc_8.radio_max = g.rc_8.radio_in;
|
||||
|
||||
channel_roll->radio_trim = channel_roll->radio_in;
|
||||
channel_pitch->radio_trim = channel_pitch->radio_in;
|
||||
channel_rudder->radio_trim = channel_rudder->radio_in;
|
||||
g.rc_5.radio_trim = 1500;
|
||||
g.rc_6.radio_trim = 1500;
|
||||
g.rc_7.radio_trim = 1500;
|
||||
g.rc_8.radio_trim = 1500;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nMove all controls to each extreme. Hit Enter to save: \n"));
|
||||
while(1) {
|
||||
|
||||
hal.scheduler->delay(20);
|
||||
// Filters radio input - adjust filters in the radio.pde file
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
|
||||
channel_roll->update_min_max();
|
||||
channel_pitch->update_min_max();
|
||||
channel_throttle->update_min_max();
|
||||
channel_rudder->update_min_max();
|
||||
g.rc_5.update_min_max();
|
||||
g.rc_6.update_min_max();
|
||||
g.rc_7.update_min_max();
|
||||
g.rc_8.update_min_max();
|
||||
|
||||
if(cliSerial->available() > 0) {
|
||||
while (cliSerial->available() > 0) {
|
||||
cliSerial->read();
|
||||
}
|
||||
channel_roll->save_eeprom();
|
||||
channel_pitch->save_eeprom();
|
||||
channel_throttle->save_eeprom();
|
||||
channel_rudder->save_eeprom();
|
||||
g.rc_5.save_eeprom();
|
||||
g.rc_6.save_eeprom();
|
||||
g.rc_7.save_eeprom();
|
||||
g.rc_8.save_eeprom();
|
||||
print_done();
|
||||
break;
|
||||
}
|
||||
}
|
||||
trim_radio();
|
||||
report_radio();
|
||||
return(0);
|
||||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
setup_erase(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
int c;
|
||||
|
||||
cliSerial->printf_P(PSTR("\nType 'Y' and hit Enter to erase all waypoint and parameter data, any other key to abort: "));
|
||||
|
||||
do {
|
||||
c = cliSerial->read();
|
||||
} while (-1 == c);
|
||||
|
||||
if (('y' != c) && ('Y' != c))
|
||||
return(-1);
|
||||
zero_eeprom();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
handle full accelerometer calibration via user dialog
|
||||
*/
|
||||
|
||||
static int8_t
|
||||
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
float trim_roll, trim_pitch;
|
||||
cliSerial->println_P(PSTR("Initialising gyros"));
|
||||
|
||||
ahrs.init();
|
||||
ahrs.set_fly_forward(true);
|
||||
ahrs.set_wind_estimation(true);
|
||||
|
||||
ins.init(AP_InertialSensor::COLD_START, ins_sample_rate);
|
||||
AP_InertialSensor_UserInteractStream interact(hal.console);
|
||||
bool success = ins.calibrate_accel(&interact, trim_roll, trim_pitch);
|
||||
if (success) {
|
||||
// reset ahrs's trim to suggested values from calibration routine
|
||||
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
|
||||
}
|
||||
report_ins();
|
||||
return(0);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
setup_compass(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!strcmp_P(argv[1].str, PSTR("on"))) {
|
||||
if (!compass.init()) {
|
||||
cliSerial->println_P(PSTR("Compass initialisation failed!"));
|
||||
g.compass_enabled = false;
|
||||
} else {
|
||||
g.compass_enabled = true;
|
||||
}
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
g.compass_enabled = false;
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("reset"))) {
|
||||
compass.set_and_save_offsets(0,0,0,0);
|
||||
|
||||
} else {
|
||||
cliSerial->printf_P(PSTR("\nOptions:[on,off,reset]\n"));
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
|
||||
g.compass_enabled.save();
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
// CLI reports
|
||||
/***************************************************************************/
|
||||
|
||||
static void report_radio()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Radio\n"));
|
||||
print_divider();
|
||||
// radio
|
||||
print_radio_values();
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_ins()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("INS\n"));
|
||||
print_divider();
|
||||
|
||||
print_gyro_offsets();
|
||||
print_accel_offsets_and_scaling();
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_compass()
|
||||
{
|
||||
cliSerial->print_P(PSTR("Compass: "));
|
||||
print_enabled(g.compass_enabled);
|
||||
|
||||
Vector3f offsets = compass.get_offsets();
|
||||
|
||||
// mag offsets
|
||||
cliSerial->printf_P(PSTR("Mag offsets: %4.4f, %4.4f, %4.4f\n"),
|
||||
(double)offsets.x,
|
||||
(double)offsets.y,
|
||||
(double)offsets.z);
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
/***************************************************************************/
|
||||
// CLI utilities
|
||||
/***************************************************************************/
|
||||
|
||||
static void
|
||||
print_radio_values()
|
||||
{
|
||||
cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_roll->radio_min, (int)channel_roll->radio_trim, (int)channel_roll->radio_max);
|
||||
cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)channel_pitch->radio_min, (int)channel_pitch->radio_trim, (int)channel_pitch->radio_max);
|
||||
cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)channel_throttle->radio_min, (int)channel_throttle->radio_trim, (int)channel_throttle->radio_max);
|
||||
cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)channel_rudder->radio_min, (int)channel_rudder->radio_trim, (int)channel_rudder->radio_max);
|
||||
cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
|
||||
cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
|
||||
cliSerial->printf_P(PSTR("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
|
||||
cliSerial->printf_P(PSTR("CH8: %d | %d | %d\n"), (int)g.rc_8.radio_min, (int)g.rc_8.radio_trim, (int)g.rc_8.radio_max);
|
||||
|
||||
}
|
||||
|
||||
static void
|
||||
print_done()
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
print_blanks(int16_t num)
|
||||
{
|
||||
while(num > 0) {
|
||||
num--;
|
||||
cliSerial->println();
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
print_divider(void)
|
||||
{
|
||||
for (int16_t i = 0; i < 40; i++) {
|
||||
cliSerial->printf_P(PSTR("-"));
|
||||
}
|
||||
cliSerial->println();
|
||||
}
|
||||
|
||||
static void zero_eeprom(void)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
|
||||
StorageManager::erase();
|
||||
cliSerial->printf_P(PSTR("done\n"));
|
||||
}
|
||||
|
||||
static void print_enabled(bool b)
|
||||
{
|
||||
if(b)
|
||||
cliSerial->printf_P(PSTR("en"));
|
||||
else
|
||||
cliSerial->printf_P(PSTR("dis"));
|
||||
cliSerial->printf_P(PSTR("abled\n"));
|
||||
}
|
||||
|
||||
static void
|
||||
print_accel_offsets_and_scaling(void)
|
||||
{
|
||||
Vector3f accel_offsets = ins.get_accel_offsets();
|
||||
Vector3f accel_scale = ins.get_accel_scale();
|
||||
cliSerial->printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
|
||||
(double)accel_offsets.x, // Pitch
|
||||
(double)accel_offsets.y, // Roll
|
||||
(double)accel_offsets.z, // YAW
|
||||
(double)accel_scale.x, // Pitch
|
||||
(double)accel_scale.y, // Roll
|
||||
(double)accel_scale.z); // YAW
|
||||
}
|
||||
|
||||
static void
|
||||
print_gyro_offsets(void)
|
||||
{
|
||||
Vector3f gyro_offsets = ins.get_gyro_offsets();
|
||||
cliSerial->printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
|
||||
(double)gyro_offsets.x,
|
||||
(double)gyro_offsets.y,
|
||||
(double)gyro_offsets.z);
|
||||
}
|
||||
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
|
|
@ -1,4 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/*****************************************************************************
|
||||
* The init_ardupilot function processes everything we need for an in - air restart
|
||||
* We will determine later if we are actually on the ground and process a
|
||||
|
|
@ -8,16 +11,10 @@
|
|||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
||||
// Functions called from the top-level menu
|
||||
static int8_t process_logs(uint8_t argc, const Menu::arg *argv); // in Log.pde
|
||||
static int8_t setup_mode(uint8_t argc, const Menu::arg *argv); // in setup.pde
|
||||
static int8_t test_mode(uint8_t argc, const Menu::arg *argv); // in test.cpp
|
||||
static int8_t reboot_board(uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
// This is the help function
|
||||
// PSTR is an AVR macro to read strings from flash memory
|
||||
// printf_P is a version of print_f that reads from flash memory
|
||||
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::main_menu_help(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Commands:\n"
|
||||
" logs log readback/setup mode\n"
|
||||
|
|
@ -32,24 +29,24 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
|
|||
static const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||
// command function called
|
||||
// ======= ===============
|
||||
{"logs", process_logs},
|
||||
{"setup", setup_mode},
|
||||
{"test", test_mode},
|
||||
{"reboot", reboot_board},
|
||||
{"help", main_menu_help},
|
||||
{"logs", MENU_FUNC(process_logs)},
|
||||
{"setup", MENU_FUNC(setup_mode)},
|
||||
{"test", MENU_FUNC(test_mode)},
|
||||
{"reboot", MENU_FUNC(reboot_board)},
|
||||
{"help", MENU_FUNC(main_menu_help)},
|
||||
};
|
||||
|
||||
// Create the top-level menu object.
|
||||
MENU(main_menu, THISFIRMWARE, main_menu_commands);
|
||||
|
||||
static int8_t reboot_board(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::reboot_board(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
hal.scheduler->reboot(false);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// the user wants the CLI. It never exits
|
||||
static void run_cli(AP_HAL::UARTDriver *port)
|
||||
void Plane::run_cli(AP_HAL::UARTDriver *port)
|
||||
{
|
||||
// disable the failsafe code in the CLI
|
||||
hal.scheduler->register_timer_failsafe(NULL,1);
|
||||
|
|
@ -68,7 +65,18 @@ static void run_cli(AP_HAL::UARTDriver *port)
|
|||
|
||||
#endif // CLI_ENABLED
|
||||
|
||||
static void init_ardupilot()
|
||||
|
||||
static void mavlink_delay_cb_static()
|
||||
{
|
||||
plane.mavlink_delay_cb();
|
||||
}
|
||||
|
||||
static void failsafe_check_static()
|
||||
{
|
||||
plane.failsafe_check();
|
||||
}
|
||||
|
||||
void Plane::init_ardupilot()
|
||||
{
|
||||
// initialise serial port
|
||||
serial_manager.init_console();
|
||||
|
|
@ -148,18 +156,7 @@ static void init_ardupilot()
|
|||
mavlink_system.sysid = g.sysid_this_mav;
|
||||
|
||||
#if LOGGING_ENABLED == ENABLED
|
||||
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
|
||||
if (!DataFlash.CardInserted()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
|
||||
g.log_bitmask.set(0);
|
||||
} else if (DataFlash.NeedErase()) {
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
|
||||
do_erase_logs();
|
||||
for (uint8_t i=0; i<num_gcs; i++) {
|
||||
gcs[i].reset_cli_timeout();
|
||||
}
|
||||
}
|
||||
arming.set_logging_available(DataFlash.CardInserted());
|
||||
log_init();
|
||||
#endif
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
|
|
@ -183,7 +180,7 @@ static void init_ardupilot()
|
|||
|
||||
// Register mavlink_delay_cb, which will run anytime you have
|
||||
// more than 5ms remaining in your call to hal.scheduler->delay
|
||||
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
|
||||
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
|
||||
|
||||
// give AHRS the airspeed sensor
|
||||
ahrs.set_airspeed(&airspeed);
|
||||
|
|
@ -210,7 +207,7 @@ static void init_ardupilot()
|
|||
* setup the 'main loop is dead' check. Note that this relies on
|
||||
* the RC library being initialised.
|
||||
*/
|
||||
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
|
||||
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
if (g.cli_enabled == 1) {
|
||||
|
|
@ -248,7 +245,7 @@ static void init_ardupilot()
|
|||
//********************************************************************************
|
||||
//This function does all the calibrations, etc. that we need during a ground start
|
||||
//********************************************************************************
|
||||
static void startup_ground(void)
|
||||
void Plane::startup_ground(void)
|
||||
{
|
||||
set_mode(INITIALISING);
|
||||
|
||||
|
|
@ -292,7 +289,7 @@ static void startup_ground(void)
|
|||
|
||||
// reset last heartbeat time, so we don't trigger failsafe on slow
|
||||
// startup
|
||||
failsafe.last_heartbeat_ms = millis();
|
||||
failsafe.last_heartbeat_ms = hal.scheduler->millis();
|
||||
|
||||
// we don't want writes to the serial port to cause us to pause
|
||||
// mid-flight, so set the serial ports non-blocking once we are
|
||||
|
|
@ -305,11 +302,11 @@ static void startup_ground(void)
|
|||
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
|
||||
}
|
||||
|
||||
static enum FlightMode get_previous_mode() {
|
||||
enum FlightMode Plane::get_previous_mode() {
|
||||
return previous_mode;
|
||||
}
|
||||
|
||||
static void set_mode(enum FlightMode mode)
|
||||
void Plane::set_mode(enum FlightMode mode)
|
||||
{
|
||||
if(control_mode == mode) {
|
||||
// don't switch modes if we are already in the correct mode.
|
||||
|
|
@ -440,7 +437,7 @@ static void set_mode(enum FlightMode mode)
|
|||
/*
|
||||
set_mode() wrapper for MAVLink SET_MODE
|
||||
*/
|
||||
static bool mavlink_set_mode(uint8_t mode)
|
||||
bool Plane::mavlink_set_mode(uint8_t mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case MANUAL:
|
||||
|
|
@ -463,7 +460,7 @@ static bool mavlink_set_mode(uint8_t mode)
|
|||
}
|
||||
|
||||
// exit_mode - perform any cleanup required when leaving a flight mode
|
||||
static void exit_mode(enum FlightMode mode)
|
||||
void Plane::exit_mode(enum FlightMode mode)
|
||||
{
|
||||
// stop mission when we leave auto
|
||||
if (mode == AUTO) {
|
||||
|
|
@ -473,9 +470,9 @@ static void exit_mode(enum FlightMode mode)
|
|||
}
|
||||
}
|
||||
|
||||
static void check_long_failsafe()
|
||||
void Plane::check_long_failsafe()
|
||||
{
|
||||
uint32_t tnow = millis();
|
||||
uint32_t tnow = hal.scheduler->millis();
|
||||
// only act on changes
|
||||
// -------------------
|
||||
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) {
|
||||
|
|
@ -503,7 +500,7 @@ static void check_long_failsafe()
|
|||
}
|
||||
}
|
||||
|
||||
static void check_short_failsafe()
|
||||
void Plane::check_short_failsafe()
|
||||
{
|
||||
// only act on changes
|
||||
// -------------------
|
||||
|
|
@ -521,7 +518,7 @@ static void check_short_failsafe()
|
|||
}
|
||||
|
||||
|
||||
static void startup_INS_ground(void)
|
||||
void Plane::startup_INS_ground(void)
|
||||
{
|
||||
if (g.hil_mode == 1) {
|
||||
while (barometer.get_last_update() == 0) {
|
||||
|
|
@ -542,7 +539,7 @@ static void startup_INS_ground(void)
|
|||
|
||||
if (style == AP_InertialSensor::COLD_START) {
|
||||
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
|
||||
mavlink_delay(100);
|
||||
hal.scheduler->delay(100);
|
||||
}
|
||||
|
||||
ahrs.init();
|
||||
|
|
@ -568,21 +565,21 @@ static void startup_INS_ground(void)
|
|||
|
||||
// updates the status of the notify objects
|
||||
// should be called at 50hz
|
||||
static void update_notify()
|
||||
void Plane::update_notify()
|
||||
{
|
||||
notify.update();
|
||||
}
|
||||
|
||||
static void resetPerfData(void)
|
||||
void Plane::resetPerfData(void)
|
||||
{
|
||||
mainLoop_count = 0;
|
||||
G_Dt_max = 0;
|
||||
G_Dt_min = 0;
|
||||
perf_mon_timer = millis();
|
||||
perf_mon_timer = hal.scheduler->millis();
|
||||
}
|
||||
|
||||
|
||||
static void check_usb_mux(void)
|
||||
void Plane::check_usb_mux(void)
|
||||
{
|
||||
bool usb_check = hal.gpio->usb_connected();
|
||||
if (usb_check == usb_connected) {
|
||||
|
|
@ -606,8 +603,7 @@ static void check_usb_mux(void)
|
|||
}
|
||||
|
||||
|
||||
static void
|
||||
print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
||||
void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case MANUAL:
|
||||
|
|
@ -656,7 +652,7 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
|
|||
}
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
static void print_comma(void)
|
||||
void Plane::print_comma(void)
|
||||
{
|
||||
cliSerial->print_P(PSTR(","));
|
||||
}
|
||||
|
|
@ -665,7 +661,7 @@ static void print_comma(void)
|
|||
/*
|
||||
write to a servo
|
||||
*/
|
||||
static void servo_write(uint8_t ch, uint16_t pwm)
|
||||
void Plane::servo_write(uint8_t ch, uint16_t pwm)
|
||||
{
|
||||
if (g.hil_mode==1 && !g.hil_servos) {
|
||||
if (ch < 8) {
|
||||
|
|
@ -680,7 +676,7 @@ static void servo_write(uint8_t ch, uint16_t pwm)
|
|||
/*
|
||||
should we log a message type now?
|
||||
*/
|
||||
static bool should_log(uint32_t mask)
|
||||
bool Plane::should_log(uint32_t mask)
|
||||
{
|
||||
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
||||
return false;
|
||||
|
|
@ -702,7 +698,7 @@ static bool should_log(uint32_t mask)
|
|||
send FrSky telemetry. Should be called at 5Hz by scheduler
|
||||
*/
|
||||
#if FRSKY_TELEM_ENABLED == ENABLED
|
||||
static void frsky_telemetry_send(void)
|
||||
void Plane::frsky_telemetry_send(void)
|
||||
{
|
||||
frsky_telemetry.send_frames((uint8_t)control_mode);
|
||||
}
|
||||
|
|
@ -712,7 +708,7 @@ static void frsky_telemetry_send(void)
|
|||
/*
|
||||
return throttle percentage from 0 to 100
|
||||
*/
|
||||
static uint8_t throttle_percentage(void)
|
||||
uint8_t Plane::throttle_percentage(void)
|
||||
{
|
||||
// to get the real throttle we need to use norm_output() which
|
||||
// returns a number from -1 to 1.
|
||||
|
|
@ -722,7 +718,7 @@ static uint8_t throttle_percentage(void)
|
|||
/*
|
||||
update AHRS soft arm state and log as needed
|
||||
*/
|
||||
static void change_arm_state(void)
|
||||
void Plane::change_arm_state(void)
|
||||
{
|
||||
Log_Arm_Disarm();
|
||||
hal.util->set_soft_armed(arming.is_armed() &&
|
||||
|
|
@ -737,7 +733,7 @@ static void change_arm_state(void)
|
|||
/*
|
||||
arm motors
|
||||
*/
|
||||
static bool arm_motors(AP_Arming::ArmingMethod method)
|
||||
bool Plane::arm_motors(AP_Arming::ArmingMethod method)
|
||||
{
|
||||
if (!arming.arm(method)) {
|
||||
return false;
|
||||
|
|
@ -752,7 +748,7 @@ static bool arm_motors(AP_Arming::ArmingMethod method)
|
|||
/*
|
||||
disarm motors
|
||||
*/
|
||||
static bool disarm_motors(void)
|
||||
bool Plane::disarm_motors(void)
|
||||
{
|
||||
if (!arming.disarm()) {
|
||||
return false;
|
||||
|
|
@ -1,5 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
/* Check for automatic takeoff conditions being met using the following sequence:
|
||||
* 1) Check for adequate GPS lock - if not return false
|
||||
* 2) Check the gravity compensated longitudinal acceleration against the threshold and start the timer if true
|
||||
|
|
@ -9,7 +11,7 @@
|
|||
* 6) If the time lapsed since the last timecheck is greater than 0.2 seconds, return false and reset the timer
|
||||
* NOTE : This function relies on the TECS 50Hz processing for its acceleration measure.
|
||||
*/
|
||||
static bool auto_takeoff_check(void)
|
||||
bool Plane::auto_takeoff_check(void)
|
||||
{
|
||||
// this is a more advanced check that relies on TECS
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
|
|
@ -85,7 +87,7 @@ no_launch:
|
|||
/*
|
||||
calculate desired bank angle during takeoff, setting nav_roll_cd
|
||||
*/
|
||||
static void takeoff_calc_roll(void)
|
||||
void Plane::takeoff_calc_roll(void)
|
||||
{
|
||||
if (steer_state.hold_course_cd == -1) {
|
||||
// we don't yet have a heading to hold - just level
|
||||
|
|
@ -105,7 +107,7 @@ static void takeoff_calc_roll(void)
|
|||
/*
|
||||
calculate desired pitch angle during takeoff, setting nav_pitch_cd
|
||||
*/
|
||||
static void takeoff_calc_pitch(void)
|
||||
void Plane::takeoff_calc_pitch(void)
|
||||
{
|
||||
if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
|
||||
// we have not reached rotate speed, use a target pitch of 5
|
||||
|
|
@ -134,7 +136,7 @@ static void takeoff_calc_pitch(void)
|
|||
This can be used either in auto-takeoff or in FBWA mode with
|
||||
FBWA_TDRAG_CHAN enabled
|
||||
*/
|
||||
static int8_t takeoff_tail_hold(void)
|
||||
int8_t Plane::takeoff_tail_hold(void)
|
||||
{
|
||||
bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) ||
|
||||
(control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode));
|
||||
|
|
@ -1,60 +1,36 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
|
||||
// These are function definitions so the Menu can be constructed before the functions
|
||||
// are defined below. Order matters to the compiler.
|
||||
static int8_t test_radio_pwm(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_radio(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_pressure(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_mag(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_xbee(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
static int8_t test_shell(uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
|
||||
// forward declaration to keep the compiler happy
|
||||
static void test_wp_print(const AP_Mission::Mission_Command& cmd);
|
||||
|
||||
// Creates a constant array of structs representing menu options
|
||||
// and stores them in Flash memory, not RAM.
|
||||
// User enters the string in the console to call the functions on the right.
|
||||
// See class Menu in AP_Common for implementation details
|
||||
static const struct Menu::command test_menu_commands[] PROGMEM = {
|
||||
{"pwm", test_radio_pwm},
|
||||
{"radio", test_radio},
|
||||
{"passthru", test_passthru},
|
||||
{"failsafe", test_failsafe},
|
||||
{"relay", test_relay},
|
||||
{"waypoints", test_wp},
|
||||
{"xbee", test_xbee},
|
||||
{"modeswitch", test_modeswitch},
|
||||
{"pwm", MENU_FUNC(test_radio_pwm)},
|
||||
{"radio", MENU_FUNC(test_radio)},
|
||||
{"passthru", MENU_FUNC(test_passthru)},
|
||||
{"failsafe", MENU_FUNC(test_failsafe)},
|
||||
{"relay", MENU_FUNC(test_relay)},
|
||||
{"waypoints", MENU_FUNC(test_wp)},
|
||||
{"xbee", MENU_FUNC(test_xbee)},
|
||||
{"modeswitch", MENU_FUNC(test_modeswitch)},
|
||||
|
||||
// Tests below here are for hardware sensors only present
|
||||
// when real sensors are attached or they are emulated
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
{"adc", test_adc},
|
||||
{"adc", MENU_FUNC(test_adc)},
|
||||
#endif
|
||||
{"gps", test_gps},
|
||||
{"ins", test_ins},
|
||||
{"airspeed", test_airspeed},
|
||||
{"airpressure", test_pressure},
|
||||
{"compass", test_mag},
|
||||
{"logging", test_logging},
|
||||
{"gps", MENU_FUNC(test_gps)},
|
||||
{"ins", MENU_FUNC(test_ins)},
|
||||
{"airspeed", MENU_FUNC(test_airspeed)},
|
||||
{"airpressure", MENU_FUNC(test_pressure)},
|
||||
{"compass", MENU_FUNC(test_mag)},
|
||||
{"logging", MENU_FUNC(test_logging)},
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
|
||||
{"shell", test_shell},
|
||||
{"shell", MENU_FUNC(test_shell)},
|
||||
#endif
|
||||
|
||||
};
|
||||
|
|
@ -62,21 +38,19 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||
// A Macro to create the Menu
|
||||
MENU(test_menu, "test", test_menu_commands);
|
||||
|
||||
static int8_t
|
||||
test_mode(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Test Mode\n\n"));
|
||||
test_menu.run();
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void print_hit_enter()
|
||||
void Plane::print_hit_enter()
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -105,8 +79,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_passthru(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -131,8 +104,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
|
|||
return 0;
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_radio(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -170,8 +142,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
uint8_t fail_test;
|
||||
print_hit_enter();
|
||||
|
|
@ -225,8 +196,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -248,8 +218,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
hal.scheduler->delay(1000);
|
||||
|
||||
|
|
@ -274,8 +243,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
|||
return (0);
|
||||
}
|
||||
|
||||
static void
|
||||
test_wp_print(const AP_Mission::Mission_Command& cmd)
|
||||
void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
|
||||
(int)cmd.index,
|
||||
|
|
@ -287,8 +255,7 @@ test_wp_print(const AP_Mission::Mission_Command& cmd)
|
|||
(long)cmd.content.location.lng);
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_xbee(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -306,8 +273,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -332,8 +298,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
|||
/*
|
||||
* test the dataflash is working
|
||||
*/
|
||||
static int8_t
|
||||
test_logging(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_logging(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
DataFlash.ShowDeviceInfo(cliSerial);
|
||||
return 0;
|
||||
|
|
@ -343,8 +308,7 @@ test_logging(uint8_t argc, const Menu::arg *argv)
|
|||
/*
|
||||
* run a debug shell
|
||||
*/
|
||||
static int8_t
|
||||
test_shell(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_shell(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
hal.util->run_debug_shell(cliSerial);
|
||||
return 0;
|
||||
|
|
@ -355,8 +319,7 @@ test_shell(uint8_t argc, const Menu::arg *argv)
|
|||
// tests in this section are for real sensors or sensors that have been simulated
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
|
||||
static int8_t
|
||||
test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
apm1_adc.Init();
|
||||
|
|
@ -375,8 +338,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
#endif
|
||||
|
||||
static int8_t
|
||||
test_gps(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
hal.scheduler->delay(1000);
|
||||
|
|
@ -404,8 +366,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_ins(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
//cliSerial->printf_P(PSTR("Calibrating."));
|
||||
ahrs.init();
|
||||
|
|
@ -456,8 +417,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!g.compass_enabled) {
|
||||
cliSerial->printf_P(PSTR("Compass: "));
|
||||
|
|
@ -473,7 +433,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
ahrs.set_fly_forward(true);
|
||||
ahrs.set_wind_estimation(true);
|
||||
ahrs.set_compass(&compass);
|
||||
report_compass();
|
||||
|
||||
// we need the AHRS initialised for this test
|
||||
ins.init(AP_InertialSensor::COLD_START,
|
||||
|
|
@ -533,8 +492,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
|||
//-------------------------------------------------------------------------------------------
|
||||
// real sensors that have not been simulated yet go here
|
||||
|
||||
static int8_t
|
||||
test_airspeed(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if (!airspeed.enabled()) {
|
||||
cliSerial->printf_P(PSTR("airspeed: "));
|
||||
|
|
@ -559,8 +517,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
|
||||
|
||||
static int8_t
|
||||
test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
|
||||
print_hit_enter();
|
||||
|
|
@ -586,4 +543,14 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
|
|||
}
|
||||
}
|
||||
|
||||
void Plane::print_enabled(bool b)
|
||||
{
|
||||
if (b) {
|
||||
cliSerial->printf_P(PSTR("en"));
|
||||
} else {
|
||||
cliSerial->printf_P(PSTR("dis"));
|
||||
}
|
||||
cliSerial->printf_P(PSTR("abled\n"));
|
||||
}
|
||||
|
||||
#endif // CLI_ENABLED
|
||||
Loading…
Reference in New Issue