Plane: convert from .pde to .cpp files

This commit is contained in:
Andrew Tridgell 2015-05-13 16:09:36 +10:00
parent fb97c16a84
commit 18c37935c9
29 changed files with 2663 additions and 1107 deletions

934
ArduPlane/ArduPlane.cpp Normal file
View File

@ -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();

View File

@ -1,16 +1,13 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
//**************************************************************** #include "Plane.h"
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
//****************************************************************
/* /*
get a speed scaling number for control surfaces. This is applied to 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 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. 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; float aspeed, speed_scaler;
if (ahrs.airspeed_estimate(&aspeed)) { 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 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) { if (auto_throttle_mode) {
// we're in an auto mode. Check the stick mixing flag // 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 previously set nav_roll calculates roll servo_out to try to
stabilize the plane at the given roll stabilize the plane at the given roll
*/ */
static void stabilize_roll(float speed_scaler) void Plane::stabilize_roll(float speed_scaler)
{ {
if (fly_inverted()) { if (fly_inverted()) {
// we want to fly upside down. We need to cope with wrap of // 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 previously set nav_pitch and calculates servo_out values to try to
stabilize the plane at the given attitude. 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(); int8_t force_elevator = takeoff_tail_hold();
if (force_elevator != 0) { 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, controller as it increases the influence of the users stick input,
allowing the user full deflection if needed 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; 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 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() || if (!stick_mixing_enabled() ||
control_mode == ACRO || control_mode == ACRO ||
@ -154,7 +151,7 @@ static void stabilize_stick_mixing_direct()
this gives the user control of the aircraft in stabilization modes this gives the user control of the aircraft in stabilization modes
using FBW style controls using FBW style controls
*/ */
static void stabilize_stick_mixing_fbw() void Plane::stabilize_stick_mixing_fbw()
{ {
if (!stick_mixing_enabled() || if (!stick_mixing_enabled() ||
control_mode == ACRO || control_mode == ACRO ||
@ -204,7 +201,7 @@ static void stabilize_stick_mixing_fbw()
- rate controlled with ground steering - rate controlled with ground steering
- yaw control for coordinated flight - 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) { if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
// in land final setup for ground steering // 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 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) { if (training_manual_roll) {
channel_roll->servo_out = channel_roll->control_in; 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 this is the ACRO mode stabilization function. It does rate
stabilization on roll and pitch axes 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 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; 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 main stabilization function for all 3 axes
*/ */
static void stabilize() void Plane::stabilize()
{ {
if (control_mode == MANUAL) { if (control_mode == MANUAL) {
// nothing to do // nothing to do
@ -391,7 +388,7 @@ static void stabilize()
} }
static void calc_throttle() void Plane::calc_throttle()
{ {
if (aparm.throttle_cruise <= 1) { if (aparm.throttle_cruise <= 1) {
// user has asked for zero throttle - this may be done by a // 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 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; bool disable_integrator = false;
if (control_mode == STABILIZE && rudder_input != 0) { 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 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 // holding a specific navigation course on the ground. Used in
// auto-takeoff and landing // auto-takeoff and landing
@ -443,7 +440,7 @@ static void calc_nav_yaw_course(void)
/* /*
calculate yaw control for ground steering 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 && if (gps.ground_speed() < 1 &&
channel_throttle->control_in == 0 && 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 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 // 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 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(); nav_roll_cd = nav_controller->nav_roll_cd();
update_load_factor(); update_load_factor();
@ -507,7 +504,7 @@ static void calc_nav_roll()
/***************************************** /*****************************************
* Throttle slew limit * 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; uint8_t slewrate = aparm.throttle_slewrate;
if (control_mode==AUTO && auto_state.takeoff_complete == false && g.takeoff_throttle_slewrate != 0) { 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 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; uint8_t slewrate = g.flap_slewrate;
// if slew limit rate is set to zero then do not slew limit // 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 * OR
* 5 - Home location is not set * 5 - Home location is not set
*/ */
static bool suppress_throttle(void) bool Plane::suppress_throttle(void)
{ {
if (!throttle_suppressed) { if (!throttle_suppressed) {
// we've previously met a condition for unsupressing the throttle // 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 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 c1, c2;
int16_t v1, v2; 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 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) || if (!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon1) ||
!RC_Channel_aux::function_assigned(RC_Channel_aux::k_flaperon2)) { !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 * 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; int16_t last_throttle = channel_throttle->radio_out;
@ -980,20 +977,18 @@ static void set_servos(void)
RC_Channel_aux::output_ch_all(); RC_Channel_aux::output_ch_all();
} }
static bool demoing_servos; void Plane::demo_servos(uint8_t i)
static void demo_servos(uint8_t i)
{ {
while(i > 0) { while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!")); gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true; demoing_servos = true;
servo_write(1, 1400); servo_write(1, 1400);
mavlink_delay(400); hal.scheduler->delay(400);
servo_write(1, 1600); servo_write(1, 1600);
mavlink_delay(200); hal.scheduler->delay(200);
servo_write(1, 1500); servo_write(1, 1500);
demoing_servos = false; demoing_servos = false;
mavlink_delay(400); hal.scheduler->delay(400);
i--; i--;
} }
} }
@ -1004,7 +999,7 @@ static void demo_servos(uint8_t i)
automatically pitch down a little when at low throttle. It makes automatically pitch down a little when at low throttle. It makes
FBWA landings without stalling much easier. 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(); uint8_t throttle = throttle_percentage();
if (throttle < aparm.throttle_cruise) { 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 ensure that the load factor does not take us below the sustainable
airspeed airspeed
*/ */
static void update_load_factor(void) void Plane::update_load_factor(void)
{ {
float demanded_roll = fabsf(nav_roll_cd*0.01f); float demanded_roll = fabsf(nav_roll_cd*0.01f);
if (demanded_roll > 85) { if (demanded_roll > 85) {

File diff suppressed because it is too large Load Diff

View File

@ -1,33 +1,28 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash.log memory // Code to Write and Read packets from DataFlash.log memory
// Code to interact with the user to dump or erase logs // 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 // Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details // See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] PROGMEM = { static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log}, {"dump", MENU_FUNC(dump_log)},
{"erase", erase_logs}, {"erase", MENU_FUNC(erase_logs)},
{"enable", select_logs}, {"enable", MENU_FUNC(select_logs)},
{"disable", select_logs} {"disable", MENU_FUNC(select_logs)}
}; };
// A Macro to create the Menu // 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 bool Plane::print_log_menu(void)
print_log_menu(void)
{ {
cliSerial->println_P(PSTR("logs enabled: ")); cliSerial->println_P(PSTR("logs enabled: "));
@ -63,39 +58,37 @@ print_log_menu(void)
return(true); return(true);
} }
static int8_t int8_t Plane::dump_log(uint8_t argc, const Menu::arg *argv)
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_start;
uint16_t dump_log_end; uint16_t dump_log_end;
uint16_t last_log_num; uint16_t last_log_num;
// check that the requested log number can be read // 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(); last_log_num = DataFlash.find_last_log();
if (dump_log == -2) { if (dump_log_num == -2) {
DataFlash.DumpPageInfo(cliSerial); DataFlash.DumpPageInfo(cliSerial);
return(-1); return(-1);
} else if (dump_log <= 0) { } else if (dump_log_num <= 0) {
cliSerial->printf_P(PSTR("dumping all\n")); cliSerial->printf_P(PSTR("dumping all\n"));
Log_Read(0, 1, 0); Log_Read(0, 1, 0);
return(-1); return(-1);
} else if ((argc != 2) } 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")); cliSerial->printf_P(PSTR("bad log number\n"));
return(-1); return(-1);
} }
DataFlash.get_log_boundaries(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, dump_log_start, dump_log_end); Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
return 0; return 0;
} }
static int8_t int8_t Plane::erase_logs(uint8_t argc, const Menu::arg *argv)
erase_logs(uint8_t argc, const Menu::arg *argv)
{ {
in_mavlink_delay = true; in_mavlink_delay = true;
do_erase_logs(); do_erase_logs();
@ -103,8 +96,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
static int8_t int8_t Plane::select_logs(uint8_t argc, const Menu::arg *argv)
select_logs(uint8_t argc, const Menu::arg *argv)
{ {
uint32_t bits; uint32_t bits;
@ -151,8 +143,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0); return(0);
} }
static int8_t int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
process_logs(uint8_t argc, const Menu::arg *argv)
{ {
log_menu.run(); log_menu.run();
return 0; return 0;
@ -160,7 +151,7 @@ process_logs(uint8_t argc, const Menu::arg *argv)
#endif // CLI_ENABLED == ENABLED #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")); gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
DataFlash.EraseAll(); DataFlash.EraseAll();
@ -169,7 +160,7 @@ static void do_erase_logs(void)
// Write an attitude packet // 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 Vector3f targets; // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
targets.x = nav_roll_cd; targets.x = nav_roll_cd;
@ -206,11 +197,11 @@ struct PACKED log_Performance {
}; };
// Write a performance monitoring packet. Total length : 19 bytes // Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance() void Plane::Log_Write_Performance()
{ {
struct log_Performance pkt = { struct log_Performance pkt = {
LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG), 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, main_loop_count : mainLoop_count,
g_dt_max : G_Dt_max, g_dt_max : G_Dt_max,
gyro_drift_x : (int16_t)(ahrs.get_gyro_drift().x * 1000), 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 // 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 = {}; mavlink_mission_item_t mav_cmd = {};
AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd); AP_Mission::mission_cmd_to_mavlink(cmd,mav_cmd);
@ -236,7 +227,7 @@ struct PACKED log_Startup {
uint16_t command_total; uint16_t command_total;
}; };
static void Log_Write_Startup(uint8_t type) void Plane::Log_Write_Startup(uint8_t type)
{ {
struct log_Startup pkt = { struct log_Startup pkt = {
LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG),
@ -249,7 +240,7 @@ static void Log_Write_Startup(uint8_t type)
Log_Write_EntireMission(); Log_Write_EntireMission();
} }
static void Log_Write_EntireMission() void Plane::Log_Write_EntireMission()
{ {
DataFlash.Log_Write_Message_P(PSTR("New mission")); 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 // 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(); Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = { struct log_Control_Tuning pkt = {
@ -292,7 +283,7 @@ static void Log_Write_Control_Tuning()
} }
// Write a TECS tuning packet // 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); SpdHgt_Controller->log_data(DataFlash, LOG_TECS_MSG);
} }
@ -311,7 +302,7 @@ struct PACKED log_Nav_Tuning {
}; };
// Write a navigation tuning packe // Write a navigation tuning packe
static void Log_Write_Nav_Tuning() void Plane::Log_Write_Nav_Tuning()
{ {
struct log_Nav_Tuning pkt = { struct log_Nav_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG), LOG_PACKET_HEADER_INIT(LOG_NTUN_MSG),
@ -337,7 +328,7 @@ struct PACKED log_Status {
uint8_t safety; uint8_t safety;
}; };
static void Log_Write_Status() void Plane::Log_Write_Status()
{ {
struct log_Status pkt = { struct log_Status pkt = {
LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG) LOG_PACKET_HEADER_INIT(LOG_STATUS_MSG)
@ -364,7 +355,7 @@ struct PACKED log_Sonar {
}; };
// Write a sonar packet // Write a sonar packet
static void Log_Write_Sonar() void Plane::Log_Write_Sonar()
{ {
struct log_Sonar pkt = { struct log_Sonar pkt = {
LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG), LOG_PACKET_HEADER_INIT(LOG_SONAR_MSG),
@ -392,7 +383,7 @@ struct PACKED log_Optflow {
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
// Write an optical flow packet // Write an optical flow packet
static void Log_Write_Optflow() void Plane::Log_Write_Optflow()
{ {
// exit immediately if not enabled // exit immediately if not enabled
if (!optflow.enabled()) { if (!optflow.enabled()) {
@ -420,7 +411,7 @@ struct PACKED log_Arm_Disarm {
uint16_t arm_checks; uint16_t arm_checks;
}; };
static void Log_Write_Current() void Plane::Log_Write_Current()
{ {
DataFlash.Log_Write_Current(battery, channel_throttle->control_in); DataFlash.Log_Write_Current(battery, channel_throttle->control_in);
@ -428,7 +419,7 @@ static void Log_Write_Current()
DataFlash.Log_Write_Power(); DataFlash.Log_Write_Power();
} }
static void Log_Arm_Disarm() { void Plane::Log_Arm_Disarm() {
struct log_Arm_Disarm pkt = { struct log_Arm_Disarm pkt = {
LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG), LOG_PACKET_HEADER_INIT(LOG_ARM_DISARM_MSG),
time_ms : hal.scheduler->millis(), time_ms : hal.scheduler->millis(),
@ -438,29 +429,29 @@ static void Log_Arm_Disarm() {
DataFlash.WriteBlock(&pkt, sizeof(pkt)); 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); 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); DataFlash.Log_Write_IMU(ins);
} }
static void Log_Write_RC(void) void Plane::Log_Write_RC(void)
{ {
DataFlash.Log_Write_RCIN(); DataFlash.Log_Write_RCIN();
DataFlash.Log_Write_RCOUT(); DataFlash.Log_Write_RCOUT();
} }
static void Log_Write_Baro(void) void Plane::Log_Write_Baro(void)
{ {
DataFlash.Log_Write_Baro(barometer); DataFlash.Log_Write_Baro(barometer);
} }
// Write a AIRSPEED packet // Write a AIRSPEED packet
static void Log_Write_Airspeed(void) void Plane::Log_Write_Airspeed(void)
{ {
DataFlash.Log_Write_Airspeed(airspeed); DataFlash.Log_Write_Airspeed(airspeed);
} }
@ -492,7 +483,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED
// Read the DataFlash.log memory : Packet Parser // 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 cliSerial->printf_P(PSTR("\n" FIRMWARE_STRING
"\nFree RAM: %u\n"), "\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)); cliSerial->println_P(PSTR(HAL_BOARD_NAME));
DataFlash.LogReadProcess(log_num, start_page, end_page, DataFlash.LogReadProcess(log_num, start_page, end_page,
print_flight_mode, AP_HAL_MEMBERPROC(&Plane::print_flight_mode),
cliSerial); cliSerial);
} }
#endif // CLI_ENABLED #endif // CLI_ENABLED
// start a new log // start a new log
static void start_logging() void Plane::start_logging()
{ {
DataFlash.StartNewLog(); DataFlash.StartNewLog();
DataFlash.Log_Write_Message_P(PSTR(FIRMWARE_STRING)); 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 #else // LOGGING_ENABLED
// dummy functions int8_t Plane::process_logs(uint8_t argc, const Menu::arg *argv)
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) {
return 0; return 0;
} }

View File

@ -1,17 +1,19 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
/* /*
* ArduPlane parameter definitions * ArduPlane parameter definitions
* *
*/ */
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value : def} } #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) { aparm.v.vtype, name, Parameters::k_param_ ## v, &aparm.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, &g.v, {group_info : class::var_info} } #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, &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, &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 // @Param: FORMAT_VERSION
// @DisplayName: Eeprom format version number // @DisplayName: Eeprom format version number
// @Description: This value is incremented when changes are made to the eeprom format // @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" }, { 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()) { if (!AP_Param::check_var_info()) {
cliSerial->printf_P(PSTR("Bad parameter table\n")); 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); g.format_version.set_and_save(Parameters::k_format_version);
cliSerial->println_P(PSTR("done.")); cliSerial->println_P(PSTR("done."));
} else { } else {
uint32_t before = micros(); uint32_t before = hal.scheduler->micros();
// Load all auto-loaded EEPROM variables // Load all auto-loaded EEPROM variables
AP_Param::load_all(); AP_Param::load_all();
AP_Param::convert_old_parameters(&conversion_table[0], sizeof(conversion_table)/sizeof(conversion_table[0])); 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);
} }
} }

93
ArduPlane/Plane.cpp Normal file
View File

@ -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;

927
ArduPlane/Plane.h Normal file
View File

@ -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;

View File

@ -14,6 +14,8 @@
along with this program. If not, see <http://www.gnu.org/licenses/>. along with this program. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "Plane.h"
/* /*
altitude handling routines. These cope with both barometric control altitude handling routines. These cope with both barometric control
and terrain following control and terrain following control
@ -22,7 +24,7 @@
/* /*
adjust altitude target depending on mode adjust altitude target depending on mode
*/ */
static void adjust_altitude_target() void Plane::adjust_altitude_target()
{ {
if (control_mode == FLY_BY_WIRE_B || if (control_mode == FLY_BY_WIRE_B ||
control_mode == CRUISE) { control_mode == CRUISE) {
@ -55,7 +57,7 @@ static void adjust_altitude_target()
/* /*
setup for a gradual glide slope to the next waypoint, if appropriate 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, // establish the distance we are travelling to the next waypoint,
// for calculating out rate of change of altitude // for calculating out rate of change of altitude
@ -102,7 +104,7 @@ static void setup_glide_slope(void)
/* /*
return RTL altitude as AMSL altitude return RTL altitude as AMSL altitude
*/ */
static int32_t get_RTL_altitude() int32_t Plane::get_RTL_altitude()
{ {
if (g.RTL_altitude_cm < 0) { if (g.RTL_altitude_cm < 0) {
return current_loc.alt; return current_loc.alt;
@ -114,7 +116,7 @@ static int32_t get_RTL_altitude()
/* /*
return relative altitude in meters (relative to home) 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; return (current_loc.alt - home.alt) * 0.01f;
} }
@ -122,7 +124,7 @@ static float relative_altitude(void)
/* /*
return relative altitude in centimeters, absolute value 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); 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 setting up for altitude hold, such as when releasing elevator in
CRUISE mode. 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 // record altitude above sea level at the current time as our
// target altitude // 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 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(); set_target_altitude_current();
@ -171,7 +173,7 @@ static void set_target_altitude_current_adjusted(void)
/* /*
set target altitude based on a location structure 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; target_altitude.amsl_cm = loc.alt;
if (loc.flags.relative_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 return relative to home target altitude in centimeters. Used for
altitude control libraries altitude control libraries
*/ */
static int32_t relative_target_altitude_cm(void) int32_t Plane::relative_target_altitude_cm(void)
{ {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
float relative_home_height; 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 change the current target altitude by an amount in centimeters. Used
to cope with changes due to elevator in CRUISE or FBWB 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; target_altitude.amsl_cm += change_cm;
#if AP_TERRAIN_AVAILABLE #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 Note that target_altitude is setup initially based on the
destination waypoint 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); set_target_altitude_location(loc);
proportion = constrain_float(proportion, 0.0f, 1.0f); 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 constrain target altitude to be between two locations. Used to
ensure we stay within two waypoints in altitude 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) { if (loc1.alt > loc2.alt) {
target_altitude.amsl_cm = constrain_int32(target_altitude.amsl_cm, loc2.alt, loc1.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 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 #if AP_TERRAIN_AVAILABLE
float terrain_height; float terrain_height;
@ -300,7 +302,7 @@ static int32_t calc_altitude_error_cm(void)
/* /*
check for FBWB_min_altitude_cm violation 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) { if (g.FBWB_min_altitude_cm == 0) {
return; return;
@ -324,7 +326,7 @@ static void check_minimum_altitude(void)
/* /*
reset the altitude offset used for glide slopes 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; 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 destination is above the current altitude then the result is
positive. 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; 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 lower pressure altitude, if current_loc is in a low part of the
terrain terrain
*/ */
static bool above_location_current(const Location &loc) bool Plane::above_location_current(const Location &loc)
{ {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
float terrain_alt; 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 modify a destination to be setup for terrain following if
TERRAIN_FOLLOW is enabled 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 AP_TERRAIN_AVAILABLE
if (g.terrain_follow) { 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 This is useful during long flights to account for barometer changes
from the GCS, or to adjust the flying height of a long mission 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); 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, during long flights to account for barometer changes from the GCS,
or to adjust the flying height of a long mission 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; 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 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; float target_alt = next_WP_loc.alt*0.01;
if (!next_WP_loc.flags.relative_alt) { 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 work out target altitude adjustment from terrain lookahead
*/ */
static float lookahead_adjustment(void) float Plane::lookahead_adjustment(void)
{ {
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE
int32_t bearing_cd; int32_t bearing_cd;
@ -521,7 +523,7 @@ static float lookahead_adjustment(void)
meters to correct target altitude. A positive number means we need meters to correct target altitude. A positive number means we need
to ask the speed/height controller to fly higher 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) { if (hal.scheduler->millis() - rangefinder_state.last_correction_time_ms > 5000) {
// we haven't had any rangefinder data for 5s - don't use it // 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 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(); uint16_t distance_cm = rangefinder.distance_cm();
float height_estimate = 0; float height_estimate = 0;

View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
#if 0 // currently unused #if 0 // currently unused
struct DataPoint { struct DataPoint {

View File

@ -3,10 +3,12 @@
* logic for dealing with the current command in the mission and home location * 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 * 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) { if (auto_state.next_wp_no_crosstrack) {
// we should not try to cross-track for this waypoint // 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(); loiter_angle_reset();
} }
static void set_guided_WP(void) void Plane::set_guided_WP(void)
{ {
if (g.loiter_radius < 0) { if (g.loiter_radius < 0) {
loiter.direction = -1; loiter.direction = -1;
@ -96,7 +98,7 @@ static void set_guided_WP(void)
// run this at setup on the ground // run this at setup on the ground
// ------------------------------- // -------------------------------
static void init_home() void Plane::init_home()
{ {
gcs_send_text_P(SEVERITY_LOW, PSTR("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 this is called as long as we have 3D lock and the arming switch is
not pushed not pushed
*/ */
static void update_home() void Plane::update_home()
{ {
if (home_is_set == HOME_SET_NOT_LOCKED) { if (home_is_set == HOME_SET_NOT_LOCKED) {
ahrs.set_home(gps.location()); ahrs.set_home(gps.location());

View File

@ -1,35 +1,11 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declarations to make compiler happy #include "Plane.h"
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
/********************************************************************************/ /********************************************************************************/
// Command Event Handlers // Command Event Handlers
/********************************************************************************/ /********************************************************************************/
bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static bool
start_command(const AP_Mission::Mission_Command& cmd)
{ {
// log when new commands start // log when new commands start
if (should_log(MASK_LOG_CMD)) { 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. 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) { switch(cmd.id) {
@ -293,7 +269,7 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret
// Nav (Must) commands // Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
static void do_RTL(void) void Plane::do_RTL(void)
{ {
auto_state.next_wp_no_crosstrack = true; auto_state.next_wp_no_crosstrack = true;
auto_state.no_crosstrack = true; auto_state.no_crosstrack = true;
@ -316,7 +292,7 @@ static void do_RTL(void)
DataFlash.Log_Write_Mode(control_mode); 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; prev_WP_loc = current_loc;
set_next_WP(cmd.content.location); 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); 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; auto_state.commanded_go_around = false;
set_next_WP(cmd.content.location); 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) { if (cmd.content.location.flags.loiter_ccw) {
loiter.direction = -1; 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); set_next_WP(cmd.content.location);
loiter_set_direction_wp(cmd); 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); set_next_WP(cmd.content.location);
loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL; loiter.total_cd = (uint32_t)(LOWBYTE(cmd.p1)) * 36000UL;
loiter_set_direction_wp(cmd); 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); set_next_WP(cmd.content.location);
// we set start_time_ms when we reach the waypoint // 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); 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; next_WP_loc.alt = cmd.content.location.alt + home.alt;
reset_offset_altitude(); 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 //set target alt
next_WP_loc.alt = cmd.content.location.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 // Verify Nav (Must) commands
/********************************************************************************/ /********************************************************************************/
static bool verify_takeoff() bool Plane::verify_takeoff()
{ {
if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) { if (ahrs.yaw_initialised() && steer_state.hold_course_cd == -1) {
const float min_gps_speed = 5; const float min_gps_speed = 5;
@ -477,7 +453,7 @@ static bool verify_takeoff()
update navigation for normal mission waypoints. Return true when the update navigation for normal mission waypoints. Return true when the
waypoint is complete 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; steer_state.hold_course_cd = -1;
@ -521,28 +497,28 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
return false; return false;
} }
static bool verify_loiter_unlim() bool Plane::verify_loiter_unlim()
{ {
update_loiter(); update_loiter();
return false; return false;
} }
static bool verify_loiter_time() bool Plane::verify_loiter_time()
{ {
update_loiter(); update_loiter();
if (loiter.start_time_ms == 0) { if (loiter.start_time_ms == 0) {
if (nav_controller->reached_loiter_target()) { if (nav_controller->reached_loiter_target()) {
// we've reached the target, start the timer // 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")); gcs_send_text_P(SEVERITY_LOW,PSTR("verify_nav: LOITER time complete"));
return true; return true;
} }
return false; return false;
} }
static bool verify_loiter_turns() bool Plane::verify_loiter_turns()
{ {
update_loiter(); update_loiter();
if (loiter.sum_cd > loiter.total_cd) { 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 reached both the desired altitude and desired heading. The desired
altitude only needs to be reached once. altitude only needs to be reached once.
*/ */
static bool verify_loiter_to_alt() bool Plane::verify_loiter_to_alt()
{ {
update_loiter(); update_loiter();
@ -618,7 +594,7 @@ static bool verify_loiter_to_alt()
return true; return true;
} }
static bool verify_RTL() bool Plane::verify_RTL()
{ {
update_loiter(); update_loiter();
if (auto_state.wp_distance <= (uint32_t)max(g.waypoint_radius,0) || 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) { if (abs(adjusted_altitude_cm() - next_WP_loc.alt) <= 500) {
return true; return true;
@ -653,16 +629,16 @@ static bool verify_continue_and_change_alt()
// Condition (May) commands // 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 condition_value = cmd.content.delay.seconds * 1000; // convert seconds to milliseconds
} }
/* /*
process a DO_CHANGE_ALT request 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_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? 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(); 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; 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 // 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; condition_value = 0;
return true; return true;
} }
return false; return false;
} }
static bool verify_change_alt() bool Plane::verify_change_alt()
{ {
if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) || if( (condition_rate>=0 && adjusted_altitude_cm() >= condition_value) ||
(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; return false;
} }
static bool verify_within_distance() bool Plane::verify_within_distance()
{ {
if (auto_state.wp_distance < max(condition_value,0)) { if (auto_state.wp_distance < max(condition_value,0)) {
condition_value = 0; condition_value = 0;
@ -720,7 +696,7 @@ static bool verify_within_distance()
// Do (Now) commands // Do (Now) commands
/********************************************************************************/ /********************************************************************************/
static void do_loiter_at_location() void Plane::do_loiter_at_location()
{ {
if (g.loiter_radius < 0) { if (g.loiter_radius < 0) {
loiter.direction = -1; loiter.direction = -1;
@ -730,7 +706,7 @@ static void do_loiter_at_location()
next_WP_loc = current_loc; 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) 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) { if (cmd.p1 == 1 && gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
init_home(); 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 // 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 #if CAMERA == ENABLED
camera.configure_cmd(cmd); 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 // 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 #if CAMERA == ENABLED
camera.control_cmd(cmd); 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 // do_take_picture - take a picture with the camera library
static void do_take_picture() void Plane::do_take_picture()
{ {
#if CAMERA == ENABLED #if CAMERA == ENABLED
camera.trigger_pic(true); camera.trigger_pic(true);
@ -789,7 +765,7 @@ static void do_take_picture()
} }
// log_picture - log picture taken and send feedback to GCS // log_picture - log picture taken and send feedback to GCS
static void log_picture() void Plane::log_picture()
{ {
gcs_send_message(MSG_CAMERA_FEEDBACK); gcs_send_message(MSG_CAMERA_FEEDBACK);
if (should_log(MASK_LOG_CAMERA)) { 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 // 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 // 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) { if (control_mode == AUTO) {
return start_command(cmd); 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 // 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 // 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) { if (control_mode == AUTO) {
return verify_command(cmd); 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 // 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 // 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) { if (control_mode == AUTO) {
gcs_send_text_fmt(PSTR("Returning to Home")); gcs_send_text_fmt(PSTR("Returning to Home"));

View File

@ -1,11 +1,10 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declaration of verify_command to keep compiler happy #include "Plane.h"
static bool verify_command(const AP_Mission::Mission_Command& cmd);
// called by update navigation at 10Hz // called by update navigation at 10Hz
// -------------------- // --------------------
static void update_commands(void) void Plane::update_commands(void)
{ {
if(control_mode == AUTO) { if(control_mode == AUTO) {
if (home_is_set != HOME_UNSET) { if (home_is_set != HOME_UNSET) {

View File

@ -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__

View File

@ -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();
}

View File

@ -1,7 +1,8 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- 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; static bool switch_debouncer;
uint8_t switchPosition = readSwitch(); uint8_t switchPosition = readSwitch();
@ -101,7 +102,7 @@ static void read_control_switch()
#endif // CONFIG_HAL_BOARD #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); uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition if (pulsewidth <= 900 || pulsewidth >= 2200) return 255; // This is an error condition
@ -113,7 +114,7 @@ static uint8_t readSwitch(void)
return 0; return 0;
} }
static void reset_control_switch() void Plane::reset_control_switch()
{ {
oldSwitchPosition = 254; oldSwitchPosition = 254;
read_control_switch(); read_control_switch();
@ -122,7 +123,7 @@ static void reset_control_switch()
/* /*
called when entering autotune called when entering autotune
*/ */
static void autotune_start(void) void Plane::autotune_start(void)
{ {
rollController.autotune_start(); rollController.autotune_start();
pitchController.autotune_start(); pitchController.autotune_start();
@ -131,7 +132,7 @@ static void autotune_start(void)
/* /*
called when exiting autotune called when exiting autotune
*/ */
static void autotune_restore(void) void Plane::autotune_restore(void)
{ {
rollController.autotune_restore(); rollController.autotune_restore();
pitchController.autotune_restore(); pitchController.autotune_restore();
@ -140,7 +141,7 @@ static void autotune_restore(void)
/* /*
are we flying inverted? are we flying inverted?
*/ */
static bool fly_inverted(void) bool Plane::fly_inverted(void)
{ {
if (g.inverted_flight_ch != 0 && inverted_flight) { if (g.inverted_flight_ch != 0 && inverted_flight) {
// controlled with INVERTED_FLIGHT_CH // controlled with INVERTED_FLIGHT_CH

View File

@ -1,11 +1,12 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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. // This is how to handle a short loss of control signal failsafe.
failsafe.state = fstype; 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, ")); gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
switch(control_mode) 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); 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. // This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, ")); 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); 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 // We're back in radio contact
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off")); 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) { if (failsafe.low_battery) {
return; return;
@ -120,7 +121,7 @@ void low_battery_event(void)
AP_Notify::flags.failsafe_battery = true; AP_Notify::flags.failsafe_battery = true;
} }
static void update_events(void) void Plane::update_events(void)
{ {
ServoRelayEvents.update_events(); ServoRelayEvents.update_events();
} }

View File

@ -1,4 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
/* /*
* failsafe support * failsafe support
* Andrew Tridgell, December 2011 * Andrew Tridgell, December 2011
@ -13,7 +16,7 @@
* this failsafe_check function is called from the core timer interrupt * this failsafe_check function is called from the core timer interrupt
* at 1kHz. * at 1kHz.
*/ */
void failsafe_check(void) void Plane::failsafe_check(void)
{ {
static uint16_t last_mainLoop_count; static uint16_t last_mainLoop_count;
static uint32_t last_timestamp; static uint32_t last_timestamp;

View File

@ -4,6 +4,8 @@
* Andrew Tridgell, December 2011 * Andrew Tridgell, December 2011
*/ */
#include "Plane.h"
#if GEOFENCE_ENABLED == ENABLED #if GEOFENCE_ENABLED == ENABLED
#define MIN_GEOFENCE_POINTS 5 // 3 to define a minimal polygon (triangle) #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 maximum number of fencepoints
*/ */
static uint8_t max_fencepoints(void) uint8_t Plane::max_fencepoints(void)
{ {
return min(255, fence_storage.size() / sizeof(Vector2l)); return min(255, fence_storage.size() / sizeof(Vector2l));
} }
@ -53,7 +55,7 @@ static uint8_t max_fencepoints(void)
/* /*
* fence boundaries fetch/store * fence boundaries fetch/store
*/ */
static Vector2l get_fence_point_with_index(unsigned i) Vector2l Plane::get_fence_point_with_index(unsigned i)
{ {
Vector2l ret; Vector2l ret;
@ -69,7 +71,7 @@ static Vector2l get_fence_point_with_index(unsigned i)
} }
// save a fence point // 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()) { if (i >= (unsigned)g.fence_total.get() || i >= max_fencepoints()) {
// not allowed // not allowed
@ -87,7 +89,7 @@ static void set_fence_point_with_index(Vector2l &point, unsigned i)
/* /*
* allocate and fill the geofence state structure * allocate and fill the geofence state structure
*/ */
static void geofence_load(void) void Plane::geofence_load(void)
{ {
uint8_t i; uint8_t i;
@ -147,7 +149,7 @@ failed:
* return true if a geo-fence has been uploaded and * return true if a geo-fence has been uploaded and
* FENCE_ACTION is 1 (not necessarily enabled) * 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 //require at least a return point and a triangle
//to define a geofence area: //to define a geofence area:
@ -160,7 +162,7 @@ static bool geofence_present(void)
/* /*
check FENCE_CHANNEL and update the is_pwm_enabled state 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; bool is_pwm_enabled;
if (g.fence_channel == 0) { if (g.fence_channel == 0) {
@ -188,7 +190,7 @@ static void geofence_update_pwm_enabled_state()
} }
//return true on success, false on failure //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) { if (geofence_state == NULL && enable) {
geofence_load(); geofence_load();
@ -210,7 +212,7 @@ static bool geofence_set_enabled(bool enable, GeofenceEnableReason r)
/* /*
* return true if geo-fencing is enabled * return true if geo-fencing is enabled
*/ */
static bool geofence_enabled(void) bool Plane::geofence_enabled(void)
{ {
geofence_update_pwm_enabled_state(); geofence_update_pwm_enabled_state();
@ -234,7 +236,7 @@ static bool geofence_enabled(void)
* Set floor state IF the fence is present. * Set floor state IF the fence is present.
* Return false on failure to set floor state. * 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) { if (geofence_state == NULL) {
return false; 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 * 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) { if (g.fence_maxalt <= g.fence_minalt) {
return false; return false;
@ -260,7 +262,7 @@ static bool geofence_check_minalt(void)
/* /*
* return true if we have breached the geo-fence maximum altiude * 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) { if (g.fence_maxalt <= g.fence_minalt) {
return false; return false;
@ -275,7 +277,7 @@ static bool geofence_check_maxalt(void)
/* /*
* check if we have breached the geo-fence * 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()) { if (!geofence_enabled()) {
// switch back to the chosen control mode if still in // 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. // we are outside, and have not previously triggered.
geofence_state->fence_triggered = true; geofence_state->fence_triggered = true;
geofence_state->breach_count++; geofence_state->breach_count++;
geofence_state->breach_time = millis(); geofence_state->breach_time = hal.scheduler->millis();
geofence_state->breach_type = breach_type; geofence_state->breach_type = breach_type;
#if FENCE_TRIGGERED_PIN > 0 #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 * disabled. Otherwise the aircraft may not be able to recover from
* a breach of the geo-fence * a breach of the geo-fence
*/ */
static bool geofence_stickmixing(void) { bool Plane::geofence_stickmixing(void) {
if (geofence_enabled() && if (geofence_enabled() &&
geofence_state != NULL && geofence_state != NULL &&
geofence_state->fence_triggered && 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) { if (geofence_enabled() && geofence_state != NULL) {
mavlink_msg_fence_status_send(chan, 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 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; return geofence_state ? geofence_state->fence_triggered : false;
} }
@ -457,24 +459,24 @@ static bool geofence_breached(void)
#else // GEOFENCE_ENABLED #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; return true;
} }
static bool geofence_enabled(void) { bool Plane::geofence_enabled(void) {
return false; return false;
} }
static bool geofence_present(void) { bool Plane::geofence_present(void) {
return false; return false;
} }
static bool geofence_set_enabled(bool enable, GeofenceEnableReason r) { bool Plane::geofence_set_enabled(bool enable, GeofenceEnableReason r) {
return false; return false;
} }
static bool geofence_set_floor_enabled(bool floor_enable) { bool Plane::geofence_set_floor_enabled(bool floor_enable) {
return false; return false;
} }

View File

@ -1,5 +1,7 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
/* /*
landing logic landing logic
*/ */
@ -8,7 +10,7 @@
update navigation for landing. Called when on landing approach or update navigation for landing. Called when on landing approach or
final flare final flare
*/ */
static bool verify_land() bool Plane::verify_land()
{ {
// we don't 'verify' landing in the sense that it never completes, // we don't 'verify' landing in the sense that it never completes,
// so we don't verify command completion. Instead we use this to // 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 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 && if (g.land_disarm_delay > 0 &&
auto_state.land_complete && 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, itself as that leads to discontinuities close to the landing point,
which can lead to erratic pitch control 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; 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 switch to that mission item. Returns false if no DO_LAND_START
available. available.
*/ */
static bool jump_to_landing_sequence(void) bool Plane::jump_to_landing_sequence(void)
{ {
uint16_t land_idx = mission.get_landing_sequence_start(); uint16_t land_idx = mission.get_landing_sequence_start();
if (land_idx != 0) { 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 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 pass the height above field elevation as the height above

56
ArduPlane/make.inc Normal file
View File

@ -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

View File

@ -1,8 +1,9 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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 // 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()) { switch ((AP_Navigation::ControllerType)g.nav_controller.get()) {
case AP_Navigation::CONTROLLER_L1: case AP_Navigation::CONTROLLER_L1:
@ -14,7 +15,7 @@ static void set_nav_controller(void)
/* /*
reset the total loiter angle reset the total loiter angle
*/ */
static void loiter_angle_reset(void) void Plane::loiter_angle_reset(void)
{ {
loiter.sum_cd = 0; loiter.sum_cd = 0;
loiter.total_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 update the total angle we have covered in a loiter. Used to support
commands to do N circles of loiter 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 target_bearing_cd = nav_controller->target_bearing_cd();
int32_t loiter_delta_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 // Function that will calculate the desired direction to fly and distance
//**************************************************************** //****************************************************************
static void navigate() void Plane::navigate()
{ {
// allow change of nav controller mid-flight // allow change of nav controller mid-flight
set_nav_controller(); set_nav_controller();
@ -72,7 +73,7 @@ static void navigate()
update_navigation(); update_navigation();
} }
static void calc_airspeed_errors() void Plane::calc_airspeed_errors()
{ {
float aspeed_cm = airspeed.get_airspeed_cm(); 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; 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 // Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards // 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); 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 handle CRUISE mode, locking heading to GPS course when we have
sufficient ground speed, and no aileron or rudder input sufficient ground speed, and no aileron or rudder input
*/ */
static void update_cruise() void Plane::update_cruise()
{ {
if (!cruise_state.locked_heading && if (!cruise_state.locked_heading &&
channel_roll->control_in == 0 && 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 In this mode the elevator is used to change target altitude. The
throttle is used to change target airspeed or throttle 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; static float last_elevator_input;
float 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 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); int32_t next_ground_course_cd = mission.get_next_ground_course_cd(-1);
if (next_ground_course_cd == -1) { if (next_ground_course_cd == -1) {

View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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 handle creation of PX4 mixer file, for failover to direct RC control
on failure of FMU on failure of FMU
@ -23,7 +25,7 @@
/* /*
create a mixer file given key fixed wing parameters 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); int mix_fd = open(filename, O_WRONLY|O_CREAT|O_TRUNC, 0644);
if (mix_fd == -1) { 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 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, // 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 // as it gives a template for the user to modify to create their

View File

@ -1,12 +1,14 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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 //Function that will read the radio data, limit servos and trigger a failsafe
// ---------------------------------------------------------------------------- // ----------------------------------------------------------------------------
/* /*
allow for runtime change of control channel ordering allow for runtime change of control channel ordering
*/ */
static void set_control_channels(void) void Plane::set_control_channels(void)
{ {
if (g.rudder_only) { if (g.rudder_only) {
// in rudder only mode the roll and rudder channels are the // 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 initialise RC input channels
*/ */
static void init_rc_in() void Plane::init_rc_in()
{ {
// set rc dead zones // set rc dead zones
channel_roll->set_default_dead_zone(30); channel_roll->set_default_dead_zone(30);
@ -51,7 +53,7 @@ static void init_rc_in()
/* /*
initialise RC output channels initialise RC output channels
*/ */
static void init_rc_out() void Plane::init_rc_out()
{ {
channel_roll->enable_out(); channel_roll->enable_out();
channel_pitch->enable_out(); channel_pitch->enable_out();
@ -75,7 +77,7 @@ static void init_rc_out()
} }
// check for pilot input on rudder stick for arming // 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 //TODO: ensure rudder arming disallowed during radio calibration
@ -108,7 +110,7 @@ static void rudder_arm_check()
// full right rudder starts arming counter // full right rudder starts arming counter
if (channel_rudder->control_in > 4000) { if (channel_rudder->control_in > 4000) {
uint32_t now = millis(); uint32_t now = hal.scheduler->millis();
if (rudder_arm_timer == 0 || if (rudder_arm_timer == 0 ||
now - rudder_arm_timer < 3000) { 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()) { if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->radio_in); 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()) { if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 1000 || rc_failsafe_active()) {
// we do not have valid RC input. Set all primary channel // 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(); read_radio();
int16_t trim_roll_range = (channel_roll->radio_max - channel_roll->radio_min)/5; 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(); channel_rudder->save_eeprom();
} }
static void trim_radio() void Plane::trim_radio()
{ {
for (uint8_t y = 0; y < 30; y++) { for (uint8_t y = 0; y < 30; y++) {
read_radio(); read_radio();
@ -305,7 +307,7 @@ static void trim_radio()
return true if throttle level is below throttle failsafe threshold return true if throttle level is below throttle failsafe threshold
or RC input is invalid or RC input is invalid
*/ */
static bool rc_failsafe_active(void) bool Plane::rc_failsafe_active(void)
{ {
if (!g.throttle_fs_enabled) { if (!g.throttle_fs_enabled) {
return false; return false;

View File

@ -1,6 +1,8 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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")); gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
barometer.calibrate(); barometer.calibrate();
@ -8,7 +10,7 @@ static void init_barometer(void)
gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete")); gcs_send_text_P(SEVERITY_LOW, PSTR("barometer calibration complete"));
} }
static void init_rangefinder(void) void Plane::init_rangefinder(void)
{ {
rangefinder.init(); rangefinder.init();
} }
@ -16,7 +18,7 @@ static void init_rangefinder(void)
/* /*
read the rangefinder and update height estimate read the rangefinder and update height estimate
*/ */
static void read_rangefinder(void) void Plane::read_rangefinder(void)
{ {
rangefinder.update(); rangefinder.update();
@ -29,7 +31,7 @@ static void read_rangefinder(void)
/* /*
ask airspeed sensor for a new value ask airspeed sensor for a new value
*/ */
static void read_airspeed(void) void Plane::read_airspeed(void)
{ {
if (airspeed.enabled()) { if (airspeed.enabled()) {
airspeed.read(); 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); airspeed.calibrate(in_startup);
read_airspeed(); read_airspeed();
@ -64,7 +66,7 @@ static void zero_airspeed(bool in_startup)
// read_battery - reads battery voltage and current and invokes failsafe // read_battery - reads battery voltage and current and invokes failsafe
// should be called at 10hz // should be called at 10hz
static void read_battery(void) void Plane::read_battery(void)
{ {
battery.read(); battery.read();
compass.set_current(battery.current_amps()); 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 // read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message // RC_CHANNELS_SCALED message
void read_receiver_rssi(void) void Plane::read_receiver_rssi(void)
{ {
// avoid divide by zero // avoid divide by zero
if (g.rssi_range <= 0) { if (g.rssi_range <= 0) {

81
ArduPlane/setup.cpp Normal file
View File

@ -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

View File

@ -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

View File

@ -1,4 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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 * 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 * We will determine later if we are actually on the ground and process a
@ -8,16 +11,10 @@
#if CLI_ENABLED == ENABLED #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 // This is the help function
// PSTR is an AVR macro to read strings from flash memory // PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads 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" cliSerial->printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\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 = { static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called // command function called
// ======= =============== // ======= ===============
{"logs", process_logs}, {"logs", MENU_FUNC(process_logs)},
{"setup", setup_mode}, {"setup", MENU_FUNC(setup_mode)},
{"test", test_mode}, {"test", MENU_FUNC(test_mode)},
{"reboot", reboot_board}, {"reboot", MENU_FUNC(reboot_board)},
{"help", main_menu_help}, {"help", MENU_FUNC(main_menu_help)},
}; };
// Create the top-level menu object. // Create the top-level menu object.
MENU(main_menu, THISFIRMWARE, main_menu_commands); 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); hal.scheduler->reboot(false);
return 0; return 0;
} }
// the user wants the CLI. It never exits // 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 // disable the failsafe code in the CLI
hal.scheduler->register_timer_failsafe(NULL,1); hal.scheduler->register_timer_failsafe(NULL,1);
@ -68,7 +65,18 @@ static void run_cli(AP_HAL::UARTDriver *port)
#endif // CLI_ENABLED #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 // initialise serial port
serial_manager.init_console(); serial_manager.init_console();
@ -148,18 +156,7 @@ static void init_ardupilot()
mavlink_system.sysid = g.sysid_this_mav; mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED #if LOGGING_ENABLED == ENABLED
DataFlash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0])); log_init();
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());
#endif #endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #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 // Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay // 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 // give AHRS the airspeed sensor
ahrs.set_airspeed(&airspeed); ahrs.set_airspeed(&airspeed);
@ -210,7 +207,7 @@ static void init_ardupilot()
* setup the 'main loop is dead' check. Note that this relies on * setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised. * 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 CLI_ENABLED == ENABLED
if (g.cli_enabled == 1) { 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 //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); set_mode(INITIALISING);
@ -292,7 +289,7 @@ static void startup_ground(void)
// reset last heartbeat time, so we don't trigger failsafe on slow // reset last heartbeat time, so we don't trigger failsafe on slow
// startup // 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 // 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 // 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.")); 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; return previous_mode;
} }
static void set_mode(enum FlightMode mode) void Plane::set_mode(enum FlightMode mode)
{ {
if(control_mode == mode) { if(control_mode == mode) {
// don't switch modes if we are already in the correct 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 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) { switch (mode) {
case MANUAL: 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 // 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 // stop mission when we leave auto
if (mode == 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 // only act on changes
// ------------------- // -------------------
if(failsafe.state != FAILSAFE_LONG && failsafe.state != FAILSAFE_GCS) { 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 // 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) { if (g.hil_mode == 1) {
while (barometer.get_last_update() == 0) { while (barometer.get_last_update() == 0) {
@ -542,7 +539,7 @@ static void startup_INS_ground(void)
if (style == AP_InertialSensor::COLD_START) { if (style == AP_InertialSensor::COLD_START) {
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane")); gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
mavlink_delay(100); hal.scheduler->delay(100);
} }
ahrs.init(); ahrs.init();
@ -568,21 +565,21 @@ static void startup_INS_ground(void)
// updates the status of the notify objects // updates the status of the notify objects
// should be called at 50hz // should be called at 50hz
static void update_notify() void Plane::update_notify()
{ {
notify.update(); notify.update();
} }
static void resetPerfData(void) void Plane::resetPerfData(void)
{ {
mainLoop_count = 0; mainLoop_count = 0;
G_Dt_max = 0; G_Dt_max = 0;
G_Dt_min = 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(); bool usb_check = hal.gpio->usb_connected();
if (usb_check == usb_connected) { if (usb_check == usb_connected) {
@ -606,8 +603,7 @@ static void check_usb_mux(void)
} }
static void void Plane::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
{ {
switch (mode) { switch (mode) {
case MANUAL: case MANUAL:
@ -656,7 +652,7 @@ print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
} }
#if CLI_ENABLED == ENABLED #if CLI_ENABLED == ENABLED
static void print_comma(void) void Plane::print_comma(void)
{ {
cliSerial->print_P(PSTR(",")); cliSerial->print_P(PSTR(","));
} }
@ -665,7 +661,7 @@ static void print_comma(void)
/* /*
write to a servo 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 (g.hil_mode==1 && !g.hil_servos) {
if (ch < 8) { if (ch < 8) {
@ -680,7 +676,7 @@ static void servo_write(uint8_t ch, uint16_t pwm)
/* /*
should we log a message type now? 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) { if (!(mask & g.log_bitmask) || in_mavlink_delay) {
return false; return false;
@ -702,7 +698,7 @@ static bool should_log(uint32_t mask)
send FrSky telemetry. Should be called at 5Hz by scheduler send FrSky telemetry. Should be called at 5Hz by scheduler
*/ */
#if FRSKY_TELEM_ENABLED == ENABLED #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); 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 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 // to get the real throttle we need to use norm_output() which
// returns a number from -1 to 1. // 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 update AHRS soft arm state and log as needed
*/ */
static void change_arm_state(void) void Plane::change_arm_state(void)
{ {
Log_Arm_Disarm(); Log_Arm_Disarm();
hal.util->set_soft_armed(arming.is_armed() && hal.util->set_soft_armed(arming.is_armed() &&
@ -737,7 +733,7 @@ static void change_arm_state(void)
/* /*
arm motors arm motors
*/ */
static bool arm_motors(AP_Arming::ArmingMethod method) bool Plane::arm_motors(AP_Arming::ArmingMethod method)
{ {
if (!arming.arm(method)) { if (!arming.arm(method)) {
return false; return false;
@ -752,7 +748,7 @@ static bool arm_motors(AP_Arming::ArmingMethod method)
/* /*
disarm motors disarm motors
*/ */
static bool disarm_motors(void) bool Plane::disarm_motors(void)
{ {
if (!arming.disarm()) { if (!arming.disarm()) {
return false; return false;

View File

@ -1,5 +1,7 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- 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: /* Check for automatic takeoff conditions being met using the following sequence:
* 1) Check for adequate GPS lock - if not return false * 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 * 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 * 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. * 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 // this is a more advanced check that relies on TECS
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
@ -85,7 +87,7 @@ no_launch:
/* /*
calculate desired bank angle during takeoff, setting nav_roll_cd 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) { if (steer_state.hold_course_cd == -1) {
// we don't yet have a heading to hold - just level // 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 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) { if (auto_state.highest_airspeed < g.takeoff_rotate_speed) {
// we have not reached rotate speed, use a target pitch of 5 // 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 This can be used either in auto-takeoff or in FBWA mode with
FBWA_TDRAG_CHAN enabled 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) || bool in_takeoff = ((control_mode == AUTO && !auto_state.takeoff_complete) ||
(control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode)); (control_mode == FLY_BY_WIRE_A && auto_state.fbwa_tdrag_takeoff_mode));

View File

@ -1,60 +1,36 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Plane.h"
#if CLI_ENABLED == ENABLED #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 // Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM. // and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right. // User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details // See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = { static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm}, {"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", test_radio}, {"radio", MENU_FUNC(test_radio)},
{"passthru", test_passthru}, {"passthru", MENU_FUNC(test_passthru)},
{"failsafe", test_failsafe}, {"failsafe", MENU_FUNC(test_failsafe)},
{"relay", test_relay}, {"relay", MENU_FUNC(test_relay)},
{"waypoints", test_wp}, {"waypoints", MENU_FUNC(test_wp)},
{"xbee", test_xbee}, {"xbee", MENU_FUNC(test_xbee)},
{"modeswitch", test_modeswitch}, {"modeswitch", MENU_FUNC(test_modeswitch)},
// Tests below here are for hardware sensors only present // Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated // when real sensors are attached or they are emulated
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
{"adc", test_adc}, {"adc", MENU_FUNC(test_adc)},
#endif #endif
{"gps", test_gps}, {"gps", MENU_FUNC(test_gps)},
{"ins", test_ins}, {"ins", MENU_FUNC(test_ins)},
{"airspeed", test_airspeed}, {"airspeed", MENU_FUNC(test_airspeed)},
{"airpressure", test_pressure}, {"airpressure", MENU_FUNC(test_pressure)},
{"compass", test_mag}, {"compass", MENU_FUNC(test_mag)},
{"logging", test_logging}, {"logging", MENU_FUNC(test_logging)},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{"shell", test_shell}, {"shell", MENU_FUNC(test_shell)},
#endif #endif
}; };
@ -62,21 +38,19 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
// A Macro to create the Menu // A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands); MENU(test_menu, "test", test_menu_commands);
static int8_t int8_t Plane::test_mode(uint8_t argc, const Menu::arg *argv)
test_mode(uint8_t argc, const Menu::arg *argv)
{ {
cliSerial->printf_P(PSTR("Test Mode\n\n")); cliSerial->printf_P(PSTR("Test Mode\n\n"));
test_menu.run(); test_menu.run();
return 0; return 0;
} }
static void print_hit_enter() void Plane::print_hit_enter()
{ {
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n")); cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
} }
static int8_t int8_t Plane::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -105,8 +79,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
} }
static int8_t int8_t Plane::test_passthru(uint8_t argc, const Menu::arg *argv)
test_passthru(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -131,8 +104,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
static int8_t int8_t Plane::test_radio(uint8_t argc, const Menu::arg *argv)
test_radio(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -170,8 +142,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
static int8_t int8_t Plane::test_failsafe(uint8_t argc, const Menu::arg *argv)
test_failsafe(uint8_t argc, const Menu::arg *argv)
{ {
uint8_t fail_test; uint8_t fail_test;
print_hit_enter(); print_hit_enter();
@ -225,8 +196,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
} }
} }
static int8_t int8_t Plane::test_relay(uint8_t argc, const Menu::arg *argv)
test_relay(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -248,8 +218,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
} }
} }
static int8_t int8_t Plane::test_wp(uint8_t argc, const Menu::arg *argv)
test_wp(uint8_t argc, const Menu::arg *argv)
{ {
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -274,8 +243,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
return (0); return (0);
} }
static void void Plane::test_wp_print(const AP_Mission::Mission_Command& cmd)
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"), cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)cmd.index, (int)cmd.index,
@ -287,8 +255,7 @@ test_wp_print(const AP_Mission::Mission_Command& cmd)
(long)cmd.content.location.lng); (long)cmd.content.location.lng);
} }
static int8_t int8_t Plane::test_xbee(uint8_t argc, const Menu::arg *argv)
test_xbee(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -306,8 +273,7 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
} }
static int8_t int8_t Plane::test_modeswitch(uint8_t argc, const Menu::arg *argv)
test_modeswitch(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -332,8 +298,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
/* /*
* test the dataflash is working * test the dataflash is working
*/ */
static int8_t int8_t Plane::test_logging(uint8_t argc, const Menu::arg *argv)
test_logging(uint8_t argc, const Menu::arg *argv)
{ {
DataFlash.ShowDeviceInfo(cliSerial); DataFlash.ShowDeviceInfo(cliSerial);
return 0; return 0;
@ -343,8 +308,7 @@ test_logging(uint8_t argc, const Menu::arg *argv)
/* /*
* run a debug shell * run a debug shell
*/ */
static int8_t int8_t Plane::test_shell(uint8_t argc, const Menu::arg *argv)
test_shell(uint8_t argc, const Menu::arg *argv)
{ {
hal.util->run_debug_shell(cliSerial); hal.util->run_debug_shell(cliSerial);
return 0; 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 // tests in this section are for real sensors or sensors that have been simulated
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 #if CONFIG_HAL_BOARD == HAL_BOARD_APM1
static int8_t int8_t Plane::test_adc(uint8_t argc, const Menu::arg *argv)
test_adc(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
apm1_adc.Init(); apm1_adc.Init();
@ -375,8 +338,7 @@ test_adc(uint8_t argc, const Menu::arg *argv)
} }
#endif #endif
static int8_t int8_t Plane::test_gps(uint8_t argc, const Menu::arg *argv)
test_gps(uint8_t argc, const Menu::arg *argv)
{ {
print_hit_enter(); print_hit_enter();
hal.scheduler->delay(1000); hal.scheduler->delay(1000);
@ -404,8 +366,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
} }
} }
static int8_t int8_t Plane::test_ins(uint8_t argc, const Menu::arg *argv)
test_ins(uint8_t argc, const Menu::arg *argv)
{ {
//cliSerial->printf_P(PSTR("Calibrating.")); //cliSerial->printf_P(PSTR("Calibrating."));
ahrs.init(); ahrs.init();
@ -456,8 +417,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
} }
static int8_t int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
test_mag(uint8_t argc, const Menu::arg *argv)
{ {
if (!g.compass_enabled) { if (!g.compass_enabled) {
cliSerial->printf_P(PSTR("Compass: ")); 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_fly_forward(true);
ahrs.set_wind_estimation(true); ahrs.set_wind_estimation(true);
ahrs.set_compass(&compass); ahrs.set_compass(&compass);
report_compass();
// we need the AHRS initialised for this test // we need the AHRS initialised for this test
ins.init(AP_InertialSensor::COLD_START, 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 // real sensors that have not been simulated yet go here
static int8_t int8_t Plane::test_airspeed(uint8_t argc, const Menu::arg *argv)
test_airspeed(uint8_t argc, const Menu::arg *argv)
{ {
if (!airspeed.enabled()) { if (!airspeed.enabled()) {
cliSerial->printf_P(PSTR("airspeed: ")); cliSerial->printf_P(PSTR("airspeed: "));
@ -559,8 +517,7 @@ test_airspeed(uint8_t argc, const Menu::arg *argv)
} }
static int8_t int8_t Plane::test_pressure(uint8_t argc, const Menu::arg *argv)
test_pressure(uint8_t argc, const Menu::arg *argv)
{ {
cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n")); cliSerial->printf_P(PSTR("Uncalibrated relative airpressure\n"));
print_hit_enter(); 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 #endif // CLI_ENABLED