Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Ported to AP_HAL

This commit is contained in:
Pat Hickey 2012-12-04 14:22:21 -08:00 committed by Andrew Tridgell
parent fff4e87619
commit 92b0c302f2
21 changed files with 509 additions and 517 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

174
ArduPlane/ArduPlane.pde
View File

@ -17,32 +17,21 @@
// Header includes
////////////////////////////////////////////////////////////////////////////////
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_Menu.h>
#include <AP_Param.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <I2C.h> // Wayne Truchsess I2C lib
#include <SPI.h> // Arduino SPI lib
#include <AP_Semaphore.h> // for removing conflict between optical flow and dataflash on SPI3 bus
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_AnalogSource.h> // ArduPilot Mega polymorphic analog getter
#include <AP_PeriodicProcess.h> // ArduPilot Mega TimerProcess
#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 <PID.h> // PID library
@ -50,61 +39,58 @@
#include <AP_RangeFinder.h> // Range finder library
#include <Filter.h> // Filter library
#include <AP_Buffer.h> // APM FIFO Buffer
#include <ModeFilter.h> // Mode Filter from Filter library
#include <LowPassFilter.h> // LowPassFilter class (inherits from Filter class)
#include <AP_Relay.h> // APM relay
#include <AP_Camera.h> // Photo or video camera
#include <AP_Airspeed.h>
#include <memcheck.h>
#include <APM_OBC.h>
#include <APM_Control.h>
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
// optional new controller library
#if APM_CONTROL == ENABLED
#include <APM_Control.h>
#endif
// Pre-AP_HAL compatibility
#include "compat.h"
// Configuration
#include "config.h"
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_Mount.h> // Camera/Antenna mount
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_HAL_AVR.h>
////////////////////////////////////////////////////////////////////////////////
// Serial ports
////////////////////////////////////////////////////////////////////////////////
//
// Note that FastSerial port buffers are allocated at ::begin time,
// so there is not much of a penalty to defining ports that we don't
// use.
//
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
#if TELEMETRY_UART2 == ENABLED
// solder bridge set to enable UART2 instead of USB MUX
FastSerialPort2(Serial3);
AP_HAL::BetterStream* cliSerial;
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
#else
FastSerialPort3(Serial3); // Telemetry port for APM1
const AP_HAL::HAL& hal = AP_HAL_AVR_APM1;
#endif
// port to use for command line interface
static FastSerial *cliSerial = &Serial;
////////////////////////////////////////////////////////////////////////////////
// AP_Param Loader
////////////////////////////////////////////////////////////////////////////////
// this sets up the parameter table, and sets the default values. This
// must be the first AP_Param variable declared to ensure its
// constructor runs before the constructors of the other AP_Param
// variables
AP_Param param_loader(var_info, WP_START_BYTE);
// Outback Challenge failsafe support
////////////////////////////////////////////////////////////////////////////////
// Outback Challenge Failsafe Support
////////////////////////////////////////////////////////////////////////////////
#if OBC_FAILSAFE == ENABLED
#include <APM_OBC.h>
APM_OBC obc;
#endif
@ -113,32 +99,6 @@ APM_OBC obc;
////////////////////////////////////////////////////////////////////////////////
static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
////////////////////////////////////////////////////////////////////////////////
// ISR Registry
////////////////////////////////////////////////////////////////////////////////
Arduino_Mega_ISR_Registry isr_registry;
////////////////////////////////////////////////////////////////////////////////
// APM_RC_Class Instance
////////////////////////////////////////////////////////////////////////////////
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
APM_RC_APM2 APM_RC;
#else
APM_RC_APM1 APM_RC;
#endif
////////////////////////////////////////////////////////////////////////////////
// Dataflash
////////////////////////////////////////////////////////////////////////////////
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
DataFlash_APM2 DataFlash;
#else
DataFlash_APM1 DataFlash;
#endif
////////////////////////////////////////////////////////////////////////////////
// Parameters
////////////////////////////////////////////////////////////////////////////////
@ -147,7 +107,6 @@ DataFlash_APM1 DataFlash;
//
static Parameters g;
////////////////////////////////////////////////////////////////////////////////
// prototypes
static void update_events(void);
@ -201,22 +160,22 @@ static AP_Compass_HMC5843 compass;
// real GPS selection
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
AP_GPS_Auto g_gps_driver(&Serial1, &g_gps);
AP_GPS_Auto g_gps_driver(hal.uartB, &g_gps);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NMEA
AP_GPS_NMEA g_gps_driver(&Serial1);
AP_GPS_NMEA g_gps_driver(hal.uartB);
#elif GPS_PROTOCOL == GPS_PROTOCOL_SIRF
AP_GPS_SIRF g_gps_driver(&Serial1);
AP_GPS_SIRF g_gps_driver(hal.uartB);
#elif GPS_PROTOCOL == GPS_PROTOCOL_UBLOX
AP_GPS_UBLOX g_gps_driver(&Serial1);
AP_GPS_UBLOX g_gps_driver(hal.uartB);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK
AP_GPS_MTK g_gps_driver(&Serial1);
AP_GPS_MTK g_gps_driver(hal.uartB);
#elif GPS_PROTOCOL == GPS_PROTOCOL_MTK16
AP_GPS_MTK16 g_gps_driver(&Serial1);
AP_GPS_MTK16 g_gps_driver(hal.uartB);
#elif GPS_PROTOCOL == GPS_PROTOCOL_NONE
AP_GPS_None g_gps_driver(NULL);
@ -252,32 +211,30 @@ AP_AHRS_HIL ahrs(&ins, g_gps);
#error Unrecognised HIL_MODE setting.
#endif // HIL MODE
// we always have a timer scheduler
AP_TimerProcess timer_scheduler;
////////////////////////////////////////////////////////////////////////////////
// GCS selection
////////////////////////////////////////////////////////////////////////////////
//
GCS_MAVLINK gcs0;
GCS_MAVLINK gcs3;
////////////////////////////////////////////////////////////////////////////////
// PITOT selection
// Analog Inputs
////////////////////////////////////////////////////////////////////////////////
//
#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
AP_AnalogSource_ADC pitot_analog_source( &adc,
CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0);
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0);
#endif
AP_HAL::AnalogSource *pitot_analog_source;
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
AP_AnalogSource_Arduino RSSI_pin(-1, 0.25);
AP_HAL::AnalogSource *rssi_analog_source;
AP_HAL::AnalogSource *vcc_pin;
AP_HAL::AnalogSource * batt_volt_pin;
AP_HAL::AnalogSource * batt_curr_pin;
////////////////////////////////////////////////////////////////////////////////
// Relay
////////////////////////////////////////////////////////////////////////////////
AP_Relay relay;
@ -314,7 +271,7 @@ static bool usb_connected;
enum FlightMode control_mode = INITIALISING;
// Used to maintain the state of the previous control switch position
// This is set to -1 when we need to re-read the switch
byte oldSwitchPosition;
uint8_t oldSwitchPosition;
// This is used to enable the inverted flight feature
bool inverted_flight = false;
// These are trim values used for elevon control
@ -341,7 +298,7 @@ static int16_t failsafe;
static bool ch3_failsafe;
// A timer used to help recovery from unusual attitudes. If we enter an unusual attitude
// while in autonomous flight this variable is used to hold roll at 0 for a recovery period
static byte crash_timer;
static uint8_t crash_timer;
// A timer used to track how long since we have received the last GCS heartbeat or rc override message
static uint32_t rc_override_fs_timer = 0;
@ -367,7 +324,7 @@ static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles
// 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)
static byte ground_start_count = 5;
static uint8_t ground_start_count = 5;
// Used to compute a speed estimate from the first valid gps fixes to decide if we are
// on the ground or in the air. Used to decide if a ground start is appropriate if we
// booted with an air start.
@ -401,12 +358,12 @@ static int32_t hold_course = -1; // deg * 100 dir
// There may be two active commands in Auto mode.
// This indicates the active navigation command by index number
static byte nav_command_index;
static uint8_t nav_command_index;
// This indicates the active non-navigation command by index number
static byte non_nav_command_index;
static uint8_t non_nav_command_index;
// This is the command type (eg navigate to waypoint) of the active navigation command
static byte nav_command_ID = NO_COMMAND;
static byte non_nav_command_ID = NO_COMMAND;
static uint8_t nav_command_ID = NO_COMMAND;
static uint8_t non_nav_command_ID = NO_COMMAND;
////////////////////////////////////////////////////////////////////////////////
// Airspeed
@ -473,7 +430,7 @@ static float current_total1;
////////////////////////////////////////////////////////////////////////////////
// Airspeed Sensors
////////////////////////////////////////////////////////////////////////////////
AP_Airspeed airspeed(&pitot_analog_source);
AP_Airspeed airspeed;
////////////////////////////////////////////////////////////////////////////////
// Altitude Sensor variables
@ -645,16 +602,16 @@ static uint16_t mainLoop_count;
static uint32_t medium_loopTimer_ms;
// Counters for branching from main control loop to slower loops
static byte medium_loopCounter;
static uint8_t medium_loopCounter;
// Number of milliseconds used in last medium loop cycle
static uint8_t delta_ms_medium_loop;
// Counters for branching from medium control loop to slower loops
static byte slow_loopCounter;
static uint8_t slow_loopCounter;
// Counter to trigger execution of very low rate processes
static byte superslow_loopCounter;
static uint8_t superslow_loopCounter;
// Counter to trigger execution of 1 Hz processes
static byte counter_one_herz;
static uint8_t counter_one_herz;
// % MCU cycles used
static float load;
@ -683,6 +640,21 @@ AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
////////////////////////////////////////////////////////////////////////////////
void setup() {
cliSerial = hal.console;
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE, 0.25);
#if CONFIG_PITOT_SOURCE == PITOT_SOURCE_ADC
pitot_analog_source = new AP_ADC_AnalogSource( &adc,
CONFIG_PITOT_SOURCE_ADC_CHANNEL, 1.0);
#elif CONFIG_PITOT_SOURCE == PITOT_SOURCE_ANALOG_PIN
pitot_analog_source = hal.analogin->channel(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0);
#endif
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
airspeed.init(pitot_analog_source);
memcheck_init();
init_ardupilot();
}
@ -1248,3 +1220,5 @@ static void update_alt()
//if(medium_loopCounter == 0 && slow_loopCounter == 0)
// add_altitude_data(millis() / 100, g_gps->altitude / 10);
}
AP_HAL_MAIN();

3
ArduPlane/Arduino.h Normal file
View File

@ -0,0 +1,3 @@
/* Stub Arduino.h header for use with AP_HAL. (The preprocessor will put
* #include "Arduino.h" on top no matter what, but we dont have the Arduino
* core in the compiler's path to find one.) */

20
ArduPlane/Attitude.pde
View File

@ -358,8 +358,8 @@ static void set_servos(void)
g.channel_roll.radio_out = g.channel_roll.radio_in;
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
} else {
g.channel_roll.radio_out = APM_RC.InputCh(CH_ROLL);
g.channel_pitch.radio_out = APM_RC.InputCh(CH_PITCH);
g.channel_roll.radio_out = hal.rcin->read(CH_ROLL);
g.channel_pitch.radio_out = hal.rcin->read(CH_PITCH);
}
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
g.channel_rudder.radio_out = g.channel_rudder.radio_in;
@ -503,10 +503,10 @@ static void set_servos(void)
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output
// ----------------------------------------
APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos
hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos
// Route configurable aux. functions to their respective servos
g.rc_5.output_ch(CH_5);
g.rc_6.output_ch(CH_6);
@ -522,17 +522,17 @@ static void set_servos(void)
static bool demoing_servos;
static void demo_servos(byte i) {
static void demo_servos(uint8_t i) {
while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true;
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
APM_RC.OutputCh(1, 1400);
hal.rcout->write(1, 1400);
mavlink_delay(400);
APM_RC.OutputCh(1, 1600);
hal.rcout->write(1, 1600);
mavlink_delay(200);
APM_RC.OutputCh(1, 1500);
hal.rcout->write(1, 1500);
#endif
demoing_servos = false;
mavlink_delay(400);

15
ArduPlane/GCS.h
View File

@ -7,10 +7,9 @@
#ifndef __GCS_H
#define __GCS_H
#include <FastSerial.h>
#include <AP_HAL.h>
#include <AP_Common.h>
#include <GPS.h>
#include <Stream.h>
#include <stdint.h>
///
@ -40,7 +39,7 @@ public:
///
/// @param port The stream over which messages are exchanged.
///
void init(FastSerial *port) {
void init(AP_HAL::UARTDriver *port) {
_port = port;
initialised = true;
}
@ -87,7 +86,7 @@ public:
protected:
/// The stream we are communicating over
FastSerial * _port;
AP_HAL::UARTDriver *_port;
};
//
@ -106,7 +105,7 @@ class GCS_MAVLINK : public GCS_Class
public:
GCS_MAVLINK();
void update(void);
void init(FastSerial *port);
void init(AP_HAL::UARTDriver *port);
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
@ -137,6 +136,8 @@ public:
// messages don't block the CPU
mavlink_statustext_t pending_status;
// call to reset the timeout window for entering the cli
void reset_cli_timeout();
private:
void handleMessage(mavlink_message_t * msg);
@ -209,6 +210,10 @@ private:
// number of extra ticks to add to slow things down for the radio
uint8_t stream_slowdown;
// millis value to calculate cli timeout relative to.
// exists so we can separate the cli entry time from the system start time
uint32_t _cli_timeout;
};
#endif // __GCS_H

101
ArduPlane/GCS_Mavlink.pde
View File

@ -315,14 +315,14 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
chan,
millis(),
0, // port
APM_RC.InputCh(CH_1),
APM_RC.InputCh(CH_2),
APM_RC.InputCh(CH_3),
APM_RC.InputCh(CH_4),
APM_RC.InputCh(CH_5),
APM_RC.InputCh(CH_6),
APM_RC.InputCh(CH_7),
APM_RC.InputCh(CH_8),
hal.rcin->read(CH_1),
hal.rcin->read(CH_2),
hal.rcin->read(CH_3),
hal.rcin->read(CH_4),
hal.rcin->read(CH_5),
hal.rcin->read(CH_6),
hal.rcin->read(CH_7),
hal.rcin->read(CH_8),
receiver_rssi);
}
@ -333,16 +333,16 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
chan,
micros(),
0, // port
APM_RC.OutputCh_current(0),
APM_RC.OutputCh_current(1),
APM_RC.OutputCh_current(2),
APM_RC.OutputCh_current(3),
APM_RC.OutputCh_current(4),
APM_RC.OutputCh_current(5),
APM_RC.OutputCh_current(6),
APM_RC.OutputCh_current(7));
hal.rcout->read(0),
hal.rcout->read(1),
hal.rcout->read(2),
hal.rcout->read(3),
hal.rcout->read(4),
hal.rcout->read(5),
hal.rcout->read(6),
hal.rcout->read(7));
#else
extern RC_Channel* rc_ch[NUM_CHANNELS];
extern RC_Channel* rc_ch[8];
mavlink_msg_servo_output_raw_send(
chan,
micros(),
@ -460,7 +460,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
#ifdef DESKTOP_BUILD
0);
#else
I2c.lockup_count());
hal.i2c->lockup_count());
#endif
}
@ -762,10 +762,10 @@ GCS_MAVLINK::GCS_MAVLINK() :
}
void
GCS_MAVLINK::init(FastSerial * port)
GCS_MAVLINK::init(AP_HAL::UARTDriver *port)
{
GCS_Class::init(port);
if (port == &Serial) {
if (port == (AP_HAL::BetterStream*)hal.uartA) {
mavlink_comm_0_port = port;
chan = MAVLINK_COMM_0;
}else{
@ -773,6 +773,7 @@ GCS_MAVLINK::init(FastSerial * port)
chan = MAVLINK_COMM_1;
}
_queued_parameter = NULL;
reset_cli_timeout();
}
void
@ -791,7 +792,7 @@ GCS_MAVLINK::update(void)
#if CLI_ENABLED == ENABLED
/* allow CLI to be started by hitting enter 3 times, if no
* heartbeat packets have been received */
if (mavlink_active == 0 && millis() < 20000) {
if (mavlink_active == 0 && (millis() - _cli_timeout) < 30000) {
if (c == '\n' || c == '\r') {
crlf_count++;
} else {
@ -1117,8 +1118,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
APM_RC.enable_out(packet.param1 - 1);
APM_RC.OutputCh(packet.param1 - 1, packet.param2);
hal.rcout->enable_ch(packet.param1 - 1);
hal.rcout->write(packet.param1 - 1, packet.param2);
result = MAV_RESULT_ACCEPTED;
break;
@ -1768,7 +1769,9 @@ mission_failed:
v[5] = packet.chan6_raw;
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
rc_override_active = APM_RC.setHIL(v);
hal.rcin->set_overrides(v, 8);
rc_override_fs_timer = millis();
break;
}
@ -1990,48 +1993,40 @@ GCS_MAVLINK::queued_waypoint_send()
}
}
void GCS_MAVLINK::reset_cli_timeout() {
_cli_timeout = millis();
}
/*
* a delay() callback that processes MAVLink packets. We set this as the
* callback in long running library initialisation routines to allow
* MAVLink to process packets while waiting for the initialisation to
* complete
*/
static void mavlink_delay(unsigned long t)
static void mavlink_delay_cb()
{
uint32_t tstart;
static uint32_t last_1hz, last_50hz, last_5s;
if (in_mavlink_delay) {
// this should never happen, but let's not tempt fate by
// letting the stack grow too much
delay(t);
return;
}
if (!gcs0.initialised) return;
in_mavlink_delay = true;
tstart = millis();
do {
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_data_stream_send();
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
}
delay(1);
uint32_t tnow = millis();
if (tnow - last_1hz > 1000) {
last_1hz = tnow;
gcs_send_message(MSG_HEARTBEAT);
gcs_send_message(MSG_EXTENDED_STATUS1);
}
if (tnow - last_50hz > 20) {
last_50hz = tnow;
gcs_update();
gcs_data_stream_send();
}
if (tnow - last_5s > 5000) {
last_5s = tnow;
gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM..."));
}
#if USB_MUX_PIN > 0
check_usb_mux();
check_usb_mux();
#endif
} while (millis() - tstart < t);
in_mavlink_delay = false;
}

334
ArduPlane/Log.pde
View File

@ -2,7 +2,7 @@
#if LOGGING_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Code to Write and Read packets from hal.dataflash->log memory
// Code to interact with the user to dump or erase logs
#define HEAD_BYTE1 0xA3 // Decimal 163
@ -51,20 +51,20 @@ print_log_menu(void)
int16_t log_start;
int16_t log_end;
int16_t temp;
int16_t last_log_num = DataFlash.find_last_log();
int16_t last_log_num = hal.dataflash->find_last_log();
uint16_t num_logs = DataFlash.get_num_logs();
uint16_t num_logs = hal.dataflash->get_num_logs();
cliSerial->printf_P(PSTR("logs enabled: "));
cliSerial->println_P(PSTR("logs enabled: "));
if (0 == g.log_bitmask) {
cliSerial->printf_P(PSTR("none"));
cliSerial->println_P(PSTR("none"));
}else{
// Macro to make the following code a bit easier on the eye.
// Pass it the capitalised name of the log option, as defined
// in defines.h but without the LOG_ prefix. It will check for
// the bit being set and print the name of the log option to suit.
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(# _s))
#define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) cliSerial->printf_P(PSTR(" %S"), PSTR(# _s))
PLOG(ATTITUDE_FAST);
PLOG(ATTITUDE_MED);
PLOG(GPS);
@ -88,7 +88,7 @@ print_log_menu(void)
for(int16_t i=num_logs; i>=1; i--) {
int16_t last_log_start = log_start, last_log_end = log_end;
temp = last_log_num-i+1;
DataFlash.get_log_boundaries(temp, log_start, log_end);
hal.dataflash->get_log_boundaries(temp, log_start, log_end);
cliSerial->printf_P(PSTR("Log %d, start %d, end %d\n"), (int)temp, (int)log_start, (int)log_end);
if (last_log_start == log_start && last_log_end == log_end) {
// we are printing bogus logs
@ -110,26 +110,29 @@ dump_log(uint8_t argc, const Menu::arg *argv)
// check that the requested log number can be read
dump_log = argv[1].i;
last_log_num = DataFlash.find_last_log();
last_log_num = hal.dataflash->find_last_log();
if (dump_log == -2) {
for(uint16_t count=1; count<=DataFlash.df_NumPages; count++) {
DataFlash.StartRead(count);
for(uint16_t count=1; count<=hal.dataflash->num_pages(); count++) {
hal.dataflash->start_read(count);
cliSerial->printf_P(PSTR("DF page, log file #, log page: %d,\t"), (int)count);
cliSerial->printf_P(PSTR("%d,\t"), (int)DataFlash.GetFileNumber());
cliSerial->printf_P(PSTR("%d\n"), (int)DataFlash.GetFilePage());
cliSerial->printf_P(PSTR("%d,\t"), (int)hal.dataflash->get_file());
cliSerial->printf_P(PSTR("%d\n"), (int)hal.dataflash->get_file_page());
}
return(-1);
} else if (dump_log <= 0) {
cliSerial->printf_P(PSTR("dumping all\n"));
Log_Read(1, DataFlash.df_NumPages);
Log_Read(1, hal.dataflash->num_pages());
return(-1);
} else if ((argc != 2) || (dump_log <= (last_log_num - DataFlash.get_num_logs())) || (dump_log > last_log_num)) {
} else if ((argc != 2)
|| (dump_log <= (last_log_num - hal.dataflash->get_num_logs()))
|| (dump_log > last_log_num))
{
cliSerial->printf_P(PSTR("bad log number\n"));
return(-1);
}
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
hal.dataflash->get_log_boundaries(dump_log, dump_log_start, dump_log_end);
cliSerial->printf_P(PSTR("Dumping Log %d, start pg %d, end pg %d\n"),
(int)dump_log,
(int)dump_log_start,
@ -143,7 +146,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
static void do_erase_logs(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Erasing logs"));
DataFlash.EraseAll(mavlink_delay);
hal.dataflash->erase_all();
gcs_send_text_P(SEVERITY_LOW, PSTR("Log erase complete"));
}
@ -212,61 +215,61 @@ process_logs(uint8_t argc, const Menu::arg *argv)
// Write an attitude packet. Total length : 10 bytes
static void Log_Write_Attitude(int16_t log_roll, int16_t log_pitch, uint16_t log_yaw)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_ATTITUDE_MSG);
DataFlash.WriteInt(log_roll);
DataFlash.WriteInt(log_pitch);
DataFlash.WriteInt(log_yaw);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_ATTITUDE_MSG);
hal.dataflash->write_word(log_roll);
hal.dataflash->write_word(log_pitch);
hal.dataflash->write_word(log_yaw);
hal.dataflash->write_byte(END_BYTE);
}
// Write a performance monitoring packet. Total length : 19 bytes
static void Log_Write_Performance()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt((int16_t)mainLoop_count);
DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(0);
DataFlash.WriteByte(0);
DataFlash.WriteByte(ahrs.renorm_range_count);
DataFlash.WriteByte(ahrs.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt(1); // AHRS health
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_PERFORMANCE_MSG);
hal.dataflash->write_dword(millis()- perf_mon_timer);
hal.dataflash->write_word((int16_t)mainLoop_count);
hal.dataflash->write_word(G_Dt_max);
hal.dataflash->write_byte(0);
hal.dataflash->write_byte(0);
hal.dataflash->write_byte(ahrs.renorm_range_count);
hal.dataflash->write_byte(ahrs.renorm_blowup_count);
hal.dataflash->write_byte(gps_fix_count);
hal.dataflash->write_word(1); // AHRS health
hal.dataflash->write_word((int)(ahrs.get_gyro_drift().x * 1000));
hal.dataflash->write_word((int)(ahrs.get_gyro_drift().y * 1000));
hal.dataflash->write_word((int)(ahrs.get_gyro_drift().z * 1000));
hal.dataflash->write_word(pmTest1);
hal.dataflash->write_byte(END_BYTE);
}
// Write a command processing packet. Total length : 19 bytes
//void Log_Write_Cmd(byte num, byte id, byte p1, int32_t alt, int32_t lat, int32_t lng)
static void Log_Write_Cmd(byte num, struct Location *wp)
static void Log_Write_Cmd(uint8_t num, struct Location *wp)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CMD_MSG);
DataFlash.WriteByte(num);
DataFlash.WriteByte(wp->id);
DataFlash.WriteByte(wp->p1);
DataFlash.WriteLong(wp->alt);
DataFlash.WriteLong(wp->lat);
DataFlash.WriteLong(wp->lng);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_CMD_MSG);
hal.dataflash->write_byte(num);
hal.dataflash->write_byte(wp->id);
hal.dataflash->write_byte(wp->p1);
hal.dataflash->write_dword(wp->alt);
hal.dataflash->write_dword(wp->lat);
hal.dataflash->write_dword(wp->lng);
hal.dataflash->write_byte(END_BYTE);
}
static void Log_Write_Startup(byte type)
static void Log_Write_Startup(uint8_t type)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_STARTUP_MSG);
DataFlash.WriteByte(type);
DataFlash.WriteByte(g.command_total);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_STARTUP_MSG);
hal.dataflash->write_byte(type);
hal.dataflash->write_byte(g.command_total);
hal.dataflash->write_byte(END_BYTE);
// create a location struct to hold the temp Waypoints for printing
struct Location cmd = get_cmd_with_index(0);
@ -284,65 +287,68 @@ static void Log_Write_Control_Tuning()
{
Vector3f accel = ins.get_accel();
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt(g.channel_roll.servo_out);
DataFlash.WriteInt(nav_roll_cd);
DataFlash.WriteInt((int)ahrs.roll_sensor);
DataFlash.WriteInt((int)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch_cd);
DataFlash.WriteInt((int)ahrs.pitch_sensor);
DataFlash.WriteInt((int)(g.channel_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000));
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_CONTROL_TUNING_MSG);
hal.dataflash->write_word(g.channel_roll.servo_out);
hal.dataflash->write_word(nav_roll_cd);
hal.dataflash->write_word((int)ahrs.roll_sensor);
hal.dataflash->write_word((int)(g.channel_pitch.servo_out));
hal.dataflash->write_word((int)nav_pitch_cd);
hal.dataflash->write_word((int)ahrs.pitch_sensor);
hal.dataflash->write_word((int)(g.channel_throttle.servo_out));
hal.dataflash->write_word((int)(g.channel_rudder.servo_out));
hal.dataflash->write_word((int)(accel.y * 10000));
hal.dataflash->write_byte(END_BYTE);
}
// Write a navigation tuning packet. Total length : 18 bytes
static void Log_Write_Nav_Tuning()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG);
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor);
DataFlash.WriteInt((int16_t)wp_distance);
DataFlash.WriteInt(target_bearing_cd);
DataFlash.WriteInt(nav_bearing_cd);
DataFlash.WriteInt(altitude_error_cm);
DataFlash.WriteInt((int16_t)airspeed.get_airspeed_cm());
DataFlash.WriteInt(0); // was nav_gain_scaler
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_NAV_TUNING_MSG);
hal.dataflash->write_word((uint16_t)ahrs.yaw_sensor);
hal.dataflash->write_word((int16_t)wp_distance);
hal.dataflash->write_word(target_bearing_cd);
hal.dataflash->write_word(nav_bearing_cd);
hal.dataflash->write_word(altitude_error_cm);
hal.dataflash->write_word((int16_t)airspeed.get_airspeed_cm());
hal.dataflash->write_word(0); // was nav_gain_scaler
hal.dataflash->write_byte(END_BYTE);
}
// Write a mode packet. Total length : 5 bytes
static void Log_Write_Mode(byte mode)
static void Log_Write_Mode(uint8_t mode)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_MODE_MSG);
DataFlash.WriteByte(mode);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_MODE_MSG);
hal.dataflash->write_byte(mode);
hal.dataflash->write_byte(END_BYTE);
}
// Write an GPS packet. Total length : 30 bytes
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats)
static void Log_Write_GPS(
int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude,
int32_t log_gps_alt, int32_t log_mix_alt,
int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix,
uint8_t log_NumSats)
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_GPS_MSG);
DataFlash.WriteLong(log_Time);
DataFlash.WriteByte(log_Fix);
DataFlash.WriteByte(log_NumSats);
DataFlash.WriteLong(log_Lattitude);
DataFlash.WriteLong(log_Longitude);
DataFlash.WriteInt(0); // was sonar_alt
DataFlash.WriteLong(log_mix_alt);
DataFlash.WriteLong(log_gps_alt);
DataFlash.WriteLong(log_Ground_Speed);
DataFlash.WriteLong(log_Ground_Course);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_GPS_MSG);
hal.dataflash->write_dword(log_Time);
hal.dataflash->write_byte(log_Fix);
hal.dataflash->write_byte(log_NumSats);
hal.dataflash->write_dword(log_Lattitude);
hal.dataflash->write_dword(log_Longitude);
hal.dataflash->write_word(0); // was sonar_alt
hal.dataflash->write_dword(log_mix_alt);
hal.dataflash->write_dword(log_gps_alt);
hal.dataflash->write_dword(log_Ground_Speed);
hal.dataflash->write_dword(log_Ground_Course);
hal.dataflash->write_byte(END_BYTE);
}
// Write an raw accel/gyro data packet. Total length : 28 bytes
@ -352,40 +358,40 @@ static void Log_Write_Raw()
Vector3f accel = ins.get_accel();
gyro *= t7; // Scale up for storage as long integers
accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_RAW_MSG);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_RAW_MSG);
DataFlash.WriteLong((long)gyro.x);
DataFlash.WriteLong((long)gyro.y);
DataFlash.WriteLong((long)gyro.z);
DataFlash.WriteLong((long)accel.x);
DataFlash.WriteLong((long)accel.y);
DataFlash.WriteLong((long)accel.z);
hal.dataflash->write_dword((long)gyro.x);
hal.dataflash->write_dword((long)gyro.y);
hal.dataflash->write_dword((long)gyro.z);
hal.dataflash->write_dword((long)accel.x);
hal.dataflash->write_dword((long)accel.y);
hal.dataflash->write_dword((long)accel.z);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(END_BYTE);
}
static void Log_Write_Current()
{
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.channel_throttle.control_in);
DataFlash.WriteInt((int)(battery_voltage1 * 100.0));
DataFlash.WriteInt((int)(current_amps1 * 100.0));
DataFlash.WriteInt((int)current_total1);
DataFlash.WriteByte(END_BYTE);
hal.dataflash->write_byte(HEAD_BYTE1);
hal.dataflash->write_byte(HEAD_BYTE2);
hal.dataflash->write_byte(LOG_CURRENT_MSG);
hal.dataflash->write_word(g.channel_throttle.control_in);
hal.dataflash->write_word((int)(battery_voltage1 * 100.0));
hal.dataflash->write_word((int)(current_amps1 * 100.0));
hal.dataflash->write_word((int)current_total1);
hal.dataflash->write_byte(END_BYTE);
}
// Read a Current packet
static void Log_Read_Current()
{
cliSerial->printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
(int)DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
(int)DataFlash.ReadInt());
(int)hal.dataflash->read_word(),
((float)hal.dataflash->read_word() / 100.f),
((float)hal.dataflash->read_word() / 100.f),
(int)hal.dataflash->read_word());
}
// Read an control tuning packet
@ -395,7 +401,7 @@ static void Log_Read_Control_Tuning()
cliSerial->printf_P(PSTR("CTUN:"));
for (int16_t y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
logvar = hal.dataflash->read_word();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
cliSerial->print(logvar);
@ -409,7 +415,7 @@ static void Log_Read_Nav_Tuning()
{
int16_t d[7];
for (int8_t i=0; i<7; i++) {
d[i] = DataFlash.ReadInt();
d[i] = hal.dataflash->read_word();
}
cliSerial->printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"), // \n
d[0]/100.0,
@ -428,14 +434,14 @@ static void Log_Read_Performance()
int16_t logvar;
cliSerial->printf_P(PSTR("PM:"));
pm_time = DataFlash.ReadLong();
pm_time = hal.dataflash->read_dword();
cliSerial->print(pm_time);
print_comma();
for (int16_t y = 1; y <= 12; y++) {
if(y < 3 || y > 7) {
logvar = DataFlash.ReadInt();
logvar = hal.dataflash->read_word();
}else{
logvar = DataFlash.ReadByte();
logvar = hal.dataflash->read_byte();
}
cliSerial->print(logvar);
print_comma();
@ -446,17 +452,17 @@ static void Log_Read_Performance()
// Read a command processing packet
static void Log_Read_Cmd()
{
byte logvarb;
uint8_t logvarb;
int32_t logvarl;
cliSerial->printf_P(PSTR("CMD:"));
for(int16_t i = 1; i < 4; i++) {
logvarb = DataFlash.ReadByte();
logvarb = hal.dataflash->read_byte();
cliSerial->print(logvarb, DEC);
print_comma();
}
for(int16_t i = 1; i < 4; i++) {
logvarl = DataFlash.ReadLong();
logvarl = hal.dataflash->read_dword();
cliSerial->print(logvarl, DEC);
print_comma();
}
@ -465,7 +471,7 @@ static void Log_Read_Cmd()
static void Log_Read_Startup()
{
byte logbyte = DataFlash.ReadByte();
uint8_t logbyte = hal.dataflash->read_byte();
if (logbyte == TYPE_AIRSTART_MSG)
cliSerial->printf_P(PSTR("AIR START - "));
@ -474,16 +480,16 @@ static void Log_Read_Startup()
else
cliSerial->printf_P(PSTR("UNKNOWN STARTUP - "));
cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
cliSerial->printf_P(PSTR(" %d commands in memory\n"),(int)hal.dataflash->read_byte());
}
// Read an attitude packet
static void Log_Read_Attitude()
{
int16_t d[3];
d[0] = DataFlash.ReadInt();
d[1] = DataFlash.ReadInt();
d[2] = DataFlash.ReadInt();
d[0] = hal.dataflash->read_word();
d[1] = hal.dataflash->read_word();
d[2] = hal.dataflash->read_word();
cliSerial->printf_P(PSTR("ATT: %d, %d, %u\n"),
(int)d[0], (int)d[1],
(unsigned)d[2]);
@ -493,25 +499,25 @@ static void Log_Read_Attitude()
static void Log_Read_Mode()
{
cliSerial->printf_P(PSTR("MOD:"));
print_flight_mode(DataFlash.ReadByte());
print_flight_mode(hal.dataflash->read_byte());
}
// Read a GPS packet
static void Log_Read_GPS()
{
int32_t l[7];
byte b[2];
uint8_t b[2];
int16_t i;
l[0] = DataFlash.ReadLong();
b[0] = DataFlash.ReadByte();
b[1] = DataFlash.ReadByte();
l[1] = DataFlash.ReadLong();
l[2] = DataFlash.ReadLong();
i = DataFlash.ReadInt();
l[3] = DataFlash.ReadLong();
l[4] = DataFlash.ReadLong();
l[5] = DataFlash.ReadLong();
l[6] = DataFlash.ReadLong();
l[0] = hal.dataflash->read_dword();
b[0] = hal.dataflash->read_byte();
b[1] = hal.dataflash->read_byte();
l[1] = hal.dataflash->read_dword();
l[2] = hal.dataflash->read_dword();
i = hal.dataflash->read_word();
l[3] = hal.dataflash->read_dword();
l[4] = hal.dataflash->read_dword();
l[5] = hal.dataflash->read_dword();
l[6] = hal.dataflash->read_dword();
cliSerial->printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %d, %4.4f, %4.4f, %4.4f, %4.4f\n"),
(long)l[0], (int)b[0], (int)b[1],
l[1]/t7, l[2]/t7,
@ -525,14 +531,14 @@ static void Log_Read_Raw()
float logvar;
cliSerial->printf_P(PSTR("RAW:"));
for (int16_t y = 0; y < 6; y++) {
logvar = (float)DataFlash.ReadLong() / t7;
logvar = (float)hal.dataflash->read_dword() / t7;
cliSerial->print(logvar);
print_comma();
}
cliSerial->println();
}
// Read the DataFlash log memory : Packet Parser
// Read the hal.dataflash->log memory : Packet Parser
static void Log_Read(int16_t start_page, int16_t end_page)
{
int16_t packet_count = 0;
@ -546,7 +552,7 @@ static void Log_Read(int16_t start_page, int16_t end_page)
if(start_page > end_page)
{
packet_count = Log_Read_Process(start_page, DataFlash.df_NumPages);
packet_count = Log_Read_Process(start_page, hal.dataflash->num_pages());
packet_count += Log_Read_Process(1, end_page);
} else {
packet_count = Log_Read_Process(start_page, end_page);
@ -555,17 +561,17 @@ static void Log_Read(int16_t start_page, int16_t end_page)
cliSerial->printf_P(PSTR("Number of packets read: %d\n"), (int) packet_count);
}
// Read the DataFlash log memory : Packet Parser
// Read the hal.dataflash->log memory : Packet Parser
static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
{
byte data;
byte log_step = 0;
uint8_t data;
uint8_t log_step = 0;
int16_t page = start_page;
int16_t packet_count = 0;
DataFlash.StartRead(start_page);
hal.dataflash->start_read(start_page);
while (page < end_page && page != -1) {
data = DataFlash.ReadByte();
data = hal.dataflash->read_byte();
switch(log_step) // This is a state machine to read the packets
{
@ -634,7 +640,7 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
log_step = 0; // Restart sequence: new packet...
break;
}
page = DataFlash.GetPage();
page = hal.dataflash->get_page();
}
return packet_count;
}
@ -642,18 +648,18 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
#else // LOGGING_ENABLED
// dummy functions
static void Log_Write_Mode(byte mode) {
static void Log_Write_Mode(uint8_t mode) {
}
static void Log_Write_Startup(byte type) {
static void Log_Write_Startup(uint8_t type) {
}
static void Log_Write_Cmd(byte num, struct Location *wp) {
static void Log_Write_Cmd(uint8_t num, struct Location *wp) {
}
static void Log_Write_Current() {
}
static void Log_Write_Nav_Tuning() {
}
static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_Longitude, int32_t log_gps_alt, int32_t log_mix_alt,
int32_t log_Ground_Speed, int32_t log_Ground_Course, byte log_Fix, byte log_NumSats) {
int32_t log_Ground_Speed, int32_t log_Ground_Course, uint8_t log_Fix, uint8_t log_NumSats) {
}
static void Log_Write_Performance() {
}

9
ArduPlane/Makefile
View File

@ -12,12 +12,18 @@ hil:
hil-apm2:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
hil-apm2-nologging:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED"
hilsensors:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS"
hilsensors-apm2:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
hilsensors-apm2-nologging:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_SENSORS -DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED"
hilnocli:
make -f Makefile EXTRAFLAGS="-DHIL_MODE=HIL_MODE_ATTITUDE -DCLI_ENABLED=DISABLED"
@ -39,6 +45,9 @@ heli:
apm2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2"
apm2-nologging:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DLOGGING_ENABLED=DISABLED"
apm2-uart2:
make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DTELEMETRY_UART2=ENABLED"

6
ArduPlane/commands_logic.pde
View File

@ -577,8 +577,8 @@ static void do_set_home()
static void do_set_servo()
{
APM_RC.enable_out(next_nonnav_command.p1 - 1);
APM_RC.OutputCh(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()
@ -595,7 +595,7 @@ static void do_set_relay()
static void do_repeat_servo(uint8_t channel, uint16_t servo_value,
int16_t repeat, uint8_t delay_time)
{
extern RC_Channel *rc_ch[NUM_CHANNELS];
extern RC_Channel *rc_ch[8];
channel = channel - 1;
if (channel < 5 || channel > 8) {
// not allowed

2
ArduPlane/commands_process.pde
View File

@ -58,7 +58,7 @@ static void process_next_command()
// and loads conditional or immediate commands if applicable
struct Location temp;
byte old_index = nav_command_index;
uint8_t old_index = nav_command_index;
// these are Navigation/Must commands
// ---------------------------------

15
ArduPlane/compat.h Normal file
View File

@ -0,0 +1,15 @@
#ifndef __COMPAT_H__
#define __COMPAT_H__
#define OUTPUT GPIO_OUTPUT
#define INPUT GPIO_INPUT
#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__

37
ArduPlane/compat.pde Normal file
View File

@ -0,0 +1,37 @@
void delay(uint32_t ms)
{
hal.scheduler->delay(ms);
}
void mavlink_delay(uint32_t ms)
{
hal.scheduler->delay(ms);
}
uint32_t millis()
{
return hal.scheduler->millis();
}
uint32_t micros()
{
return hal.scheduler->micros();
}
void pinMode(uint8_t pin, uint8_t output)
{
hal.gpio->pinMode(pin, output);
}
void digitalWrite(uint8_t pin, uint8_t out)
{
hal.gpio->write(pin,out);
}
uint8_t digitalRead(uint8_t pin)
{
return hal.gpio->read(pin);
}

12
ArduPlane/control_modes.pde
View File

@ -4,7 +4,7 @@
static void read_control_switch()
{
static bool switch_debouncer;
byte switchPosition = readSwitch();
uint8_t switchPosition = readSwitch();
// If switchPosition = 255 this indicates that the mode control channel input was out of range
// If we get this value we do not want to change modes.
@ -18,7 +18,7 @@ static void read_control_switch()
// as a spring loaded trainer switch).
if (oldSwitchPosition != switchPosition ||
(g.reset_switch_chan != 0 &&
APM_RC.InputCh(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
if (switch_debouncer == false) {
// this ensures that mode switches only happen if the
@ -36,7 +36,7 @@ static void read_control_switch()
}
if (g.reset_mission_chan != 0 &&
APM_RC.InputCh(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
// reset to first waypoint in mission
prev_WP = current_loc;
change_command(0);
@ -47,12 +47,12 @@ static void read_control_switch()
if (g.inverted_flight_ch != 0) {
// if the user has configured an inverted flight channel, then
// fly upside down when that channel goes above INVERTED_FLIGHT_PWM
inverted_flight = (control_mode != MANUAL && APM_RC.InputCh(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM);
inverted_flight = (control_mode != MANUAL && hal.rcin->read(g.inverted_flight_ch-1) > INVERTED_FLIGHT_PWM);
}
}
static byte readSwitch(void){
uint16_t pulsewidth = APM_RC.InputCh(g.flight_mode_channel - 1);
static uint8_t readSwitch(void){
uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;

9
ArduPlane/events.pde
View File

@ -36,7 +36,8 @@ static void failsafe_long_on_event(int16_t fstype)
{
// This is how to handle a long loss of control signal failsafe.
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Long event on, "));
APM_RC.clearOverride(); // If the GCS is locked up we allow control to revert to RC
// If the GCS is locked up we allow control to revert to RC
hal.rcin->clear_overrides();
failsafe = fstype;
switch(control_mode)
{
@ -104,11 +105,11 @@ static void update_events(void)
switch (event_state.type) {
case EVENT_TYPE_SERVO:
APM_RC.enable_out(event_state.rc_channel);
hal.rcout->enable_ch(event_state.rc_channel);
if (event_state.repeat & 1) {
APM_RC.OutputCh(event_state.rc_channel, event_state.undo_value);
hal.rcout->write(event_state.rc_channel, event_state.undo_value);
} else {
APM_RC.OutputCh(event_state.rc_channel, event_state.servo_value);
hal.rcout->write(event_state.rc_channel, event_state.servo_value);
}
break;

4
ArduPlane/failsafe.pde
View File

@ -38,13 +38,13 @@ void failsafe_check(uint32_t tnow)
if (in_failsafe && tnow - last_timestamp > 20000) {
// pass RC inputs to outputs every 20ms
last_timestamp = tnow;
APM_RC.clearOverride();
hal.rcin->clear_overrides();
uint8_t start_ch = 0;
if (demoing_servos) {
start_ch = 1;
}
for (uint8_t ch=start_ch; ch<4; ch++) {
APM_RC.OutputCh(ch, APM_RC.InputCh(ch));
hal.rcout->write(ch, hal.rcin->read(ch));
}
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true);

4
ArduPlane/geofence.pde
View File

@ -22,7 +22,7 @@ static struct geofence_state {
uint16_t breach_count;
uint8_t breach_type;
uint32_t breach_time;
byte old_switch_position;
uint8_t old_switch_position;
/* point 0 is the return point */
Vector2l boundary[MAX_FENCEPOINTS];
} *geofence_state;
@ -129,7 +129,7 @@ static bool geofence_enabled(void)
g.fence_total < 5 ||
(g.fence_action != FENCE_ACTION_REPORT &&
(g.fence_channel == 0 ||
APM_RC.InputCh(g.fence_channel-1) < FENCE_ENABLE_PWM))) {
hal.rcin->read(g.fence_channel-1) < FENCE_ENABLE_PWM))) {
// geo-fencing is disabled
if (geofence_state != NULL) {
// re-arm for when the channel trigger is switched on

0
ArduPlane/nocore.inoflag Normal file
View File

52
ArduPlane/radio.pde
View File

@ -2,10 +2,10 @@
//Function that will read the radio data, limit servos and trigger a failsafe
// ----------------------------------------------------------------------------
static byte failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
static uint8_t failsafeCounter = 0; // we wait a second to take over the throttle and send the plane circling
extern RC_Channel* rc_ch[NUM_CHANNELS];
extern RC_Channel* rc_ch[8];
static void init_rc_in()
{
@ -45,36 +45,34 @@ static void init_rc_in()
static void init_rc_out()
{
APM_RC.Init( &isr_registry ); // APM Radio initialization
APM_RC.enable_out(CH_1);
APM_RC.enable_out(CH_2);
APM_RC.enable_out(CH_3);
APM_RC.enable_out(CH_4);
hal.rcout->enable_ch(CH_1);
hal.rcout->enable_ch(CH_2);
hal.rcout->enable_ch(CH_3);
hal.rcout->enable_ch(CH_4);
enable_aux_servos();
// Initialization of servo outputs
APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim);
APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim);
APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min);
APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim);
hal.rcout->write(CH_1, g.channel_roll.radio_trim);
hal.rcout->write(CH_2, g.channel_pitch.radio_trim);
hal.rcout->write(CH_3, g.channel_throttle.radio_min);
hal.rcout->write(CH_4, g.channel_rudder.radio_trim);
APM_RC.OutputCh(CH_5, g.rc_5.radio_trim);
APM_RC.OutputCh(CH_6, g.rc_6.radio_trim);
APM_RC.OutputCh(CH_7, g.rc_7.radio_trim);
APM_RC.OutputCh(CH_8, g.rc_8.radio_trim);
hal.rcout->write(CH_5, g.rc_5.radio_trim);
hal.rcout->write(CH_6, g.rc_6.radio_trim);
hal.rcout->write(CH_7, g.rc_7.radio_trim);
hal.rcout->write(CH_8, g.rc_8.radio_trim);
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM1
APM_RC.OutputCh(CH_9, g.rc_9.radio_trim);
APM_RC.OutputCh(CH_10, g.rc_10.radio_trim);
APM_RC.OutputCh(CH_11, g.rc_11.radio_trim);
hal.rcout->write(CH_9, g.rc_9.radio_trim);
hal.rcout->write(CH_10, g.rc_10.radio_trim);
hal.rcout->write(CH_11, g.rc_11.radio_trim);
#endif
}
static void read_radio()
{
ch1_temp = APM_RC.InputCh(CH_ROLL);
ch2_temp = APM_RC.InputCh(CH_PITCH);
ch1_temp = hal.rcin->read(CH_ROLL);
ch2_temp = hal.rcin->read(CH_PITCH);
if(g.mix_mode == 0) {
g.channel_roll.set_pwm(ch1_temp);
@ -84,12 +82,12 @@ static void read_radio()
g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500);
}
g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
g.channel_rudder.set_pwm(hal.rcin->read(CH_4));
g.rc_5.set_pwm(hal.rcin->read(CH_5));
g.rc_6.set_pwm(hal.rcin->read(CH_6));
g.rc_7.set_pwm(hal.rcin->read(CH_7));
g.rc_8.set_pwm(hal.rcin->read(CH_8));
control_failsafe(g.channel_throttle.radio_in);

18
ArduPlane/sensors.pde
View File

@ -7,7 +7,7 @@ static LowPassFilterInt32 altitude_filter;
static void init_barometer(void)
{
gcs_send_text_P(SEVERITY_LOW, PSTR("Calibrating barometer"));
barometer.calibrate(mavlink_delay);
barometer.calibrate();
// filter at 100ms sampling, with 0.7Hz cutoff frequency
altitude_filter.set_cutoff_frequency(0.1, 0.7);
@ -33,7 +33,7 @@ static void read_airspeed(void)
static void zero_airspeed(void)
{
airspeed.calibrate(mavlink_delay);
airspeed.calibrate();
gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
}
@ -45,16 +45,14 @@ static void read_battery(void)
}
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
static AP_AnalogSource_Arduino batt_volt_pin(g.battery_volt_pin);
// this copes with changing the pin at runtime
batt_volt_pin.set_pin(g.battery_volt_pin);
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin.read_average());
batt_volt_pin->set_pin(g.battery_volt_pin);
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin->read_average());
}
if(g.battery_monitoring == 4) {
static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin);
// this copes with changing the pin at runtime
batt_curr_pin.set_pin(g.battery_curr_pin);
current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average());
batt_curr_pin->set_pin(g.battery_curr_pin);
current_amps1 = CURRENT_AMPS(batt_curr_pin->read_average());
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
}
@ -69,8 +67,8 @@ static void read_battery(void)
// RC_CHANNELS_SCALED message
void read_receiver_rssi(void)
{
RSSI_pin.set_pin(g.rssi_pin);
float ret = RSSI_pin.read();
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->read_average();
receiver_rssi = constrain(ret, 0, 255);
}

34
ArduPlane/setup.pde
View File

@ -170,7 +170,9 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_8.update_min_max();
if(cliSerial->available() > 0) {
cliSerial->flush();
while (cliSerial->available() > 0) {
cliSerial->read();
}
g.channel_roll.save_eeprom();
g.channel_pitch.save_eeprom();
g.channel_throttle.save_eeprom();
@ -192,7 +194,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
static int8_t
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
{
byte switchPosition, mode = 0;
uint8_t switchPosition, mode = 0;
cliSerial->printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
print_hit_enter();
@ -306,34 +308,14 @@ setup_level(uint8_t argc, const Menu::arg *argv)
handle full accelerometer calibration via user dialog
*/
static void setup_printf_P(const prog_char_t *fmt, ...)
{
va_list arg_list;
va_start(arg_list, fmt);
cliSerial->vprintf_P(fmt, arg_list);
va_end(arg_list);
}
static void setup_wait_key(void)
{
// wait for user input
while (!cliSerial->available()) {
delay(20);
}
// clear input buffer
while( cliSerial->available() ) {
cliSerial->read();
}
}
static int8_t
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
{
cliSerial->println_P(PSTR("Initialising gyros"));
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate,
delay, flash_leds, &timer_scheduler);
if (ins.calibrate_accel(delay, flash_leds, setup_printf_P, setup_wait_key)) {
flash_leds);
if (ins.calibrate_accel(flash_leds, hal.console)) {
if (g.manual_level == 0) {
cliSerial->println_P(PSTR("Setting MANUAL_LEVEL to 1"));
g.manual_level.set_and_save(1);
@ -564,7 +546,7 @@ print_radio_values()
}
static void
print_switch(byte p, byte m)
print_switch(uint8_t p, uint8_t m)
{
cliSerial->printf_P(PSTR("Pos %d: "),p);
print_flight_mode(m);
@ -623,7 +605,7 @@ radio_input_switch(void)
static void zero_eeprom(void)
{
byte b = 0;
uint8_t b = 0;
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
for (intptr_t i = 0; i < EEPROM_MAX_ADDR; i++) {
eeprom_write_byte((uint8_t *) i, b);

117
ArduPlane/system.pde
View File

@ -49,10 +49,10 @@ static int8_t reboot_board(uint8_t argc, const Menu::arg *argv)
}
// the user wants the CLI. It never exits
static void run_cli(FastSerial *port)
static void run_cli(AP_HAL::UARTDriver *port)
{
// disable the failsafe code in the CLI
timer_scheduler.set_failsafe(NULL);
hal.scheduler->register_timer_failsafe(NULL,1);
cliSerial = port;
Menu::set_port(port);
@ -90,44 +90,20 @@ static void init_ardupilot()
// The console port buffers are defined to be sufficiently large to support
// the MAVLink protocol efficiently
//
Serial.begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
hal.uartA->begin(SERIAL0_BAUD, 128, SERIAL_BUFSIZE);
// GPS serial port.
//
// standard gps running
Serial1.begin(38400, 256, 16);
hal.uartB->begin(38400, 256, 16);
cliSerial->printf_P(PSTR("\n\nInit " THISFIRMWARE
"\n\nFree RAM: %u\n"),
memcheck_available_memory());
//
// Initialize Wire and SPI libraries
//
#ifndef DESKTOP_BUILD
I2c.begin();
I2c.timeOut(5);
// initially set a fast I2c speed, and drop it on first failures
I2c.setSpeed(true);
#endif
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate
//
// Initialize the ISR registry.
//
isr_registry.init();
//
// Initialize the timer scheduler to use the ISR registry.
//
timer_scheduler.init( &isr_registry );
// initialise the analog port reader
AP_AnalogSource_Arduino::init_timer(&timer_scheduler);
//
// Check the EEPROM format version before loading any parameters from EEPROM.
// Check the EEPROM format version before loading any parameters from EEPROM
//
load_parameters();
@ -136,46 +112,47 @@ static void init_ardupilot()
g.num_resets.set_and_save(g.num_resets+1);
// init the GCS
gcs0.init(&Serial);
gcs0.init(hal.uartA);
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
#if USB_MUX_PIN > 0
if (!usb_connected) {
// we are not connected via USB, re-init UART0 with right
// baud rate
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
#else
// we have a 2nd serial port for telemetry
Serial3.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD), 128, SERIAL_BUFSIZE);
gcs3.init(&Serial3);
hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
128, SERIAL_BUFSIZE);
gcs3.init(hal.uartC);
#endif
mavlink_system.sysid = g.sysid_this_mav;
#if LOGGING_ENABLED == ENABLED
DataFlash.Init(); // DataFlash log initialization
if (!DataFlash.CardInserted()) {
if (!hal.dataflash->media_present()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("No dataflash card inserted"));
g.log_bitmask.set(0);
} else if (DataFlash.NeedErase()) {
} else if (hal.dataflash->need_erase()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("ERASING LOGS"));
do_erase_logs();
gcs0.reset_cli_timeout();
}
if (g.log_bitmask != 0) {
DataFlash.start_new_log();
hal.dataflash->start_new_log();
}
#endif
#if HIL_MODE != HIL_MODE_ATTITUDE
#if CONFIG_ADC == ENABLED
adc.Init(&timer_scheduler); // APM ADC library initialization
adc.Init(); // APM ADC library initialization
#endif
// initialise the analog port reader
AP_AnalogSource_Arduino::init_timer(&timer_scheduler);
barometer.init(&timer_scheduler);
barometer.init();
if (g.compass_enabled==true) {
compass.set_orientation(MAG_ORIENTATION); // set compass's orientation on aircraft
@ -207,9 +184,9 @@ static void init_ardupilot()
mavlink_system.compid = 1; //MAV_COMP_ID_IMU; // We do not check for comp id
mavlink_system.type = MAV_TYPE_FIXED_WING;
rc_override_active = APM_RC.setHIL(rc_override); // Set initial values for no override
// Set initial values for no override
rc_override_active = hal.rcin->set_overrides(rc_override, 8);
RC_Channel::set_apm_rc( &APM_RC ); // Provide reference to RC outputs.
init_rc_in(); // sets up rc channels from radio
init_rc_out(); // sets up the timer libs
@ -217,7 +194,7 @@ static void init_ardupilot()
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
#if CONFIG_RELAY == ENABLED
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
relay.init();
#endif
#if FENCE_TRIGGERED_PIN > 0
@ -229,12 +206,12 @@ static void init_ardupilot()
* setup the 'main loop is dead' check. Note that this relies on
* the RC library being initialised.
*/
timer_scheduler.set_failsafe(failsafe_check);
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
const prog_char_t *msg = PSTR("\nPress ENTER 3 times to start interactive setup\n");
cliSerial->println_P(msg);
#if USB_MUX_PIN == 0
Serial3.println_P(msg);
hal.uartC->println_P(msg);
#endif
if (ENABLE_AIR_START == 1) {
@ -247,19 +224,21 @@ static void init_ardupilot()
#if HIL_MODE != HIL_MODE_ATTITUDE
ins.init(AP_InertialSensor::WARM_START,
ins_sample_rate,
mavlink_delay, flash_leds, &timer_scheduler);
flash_leds);
ahrs.init(&timer_scheduler);
ahrs.init();
ahrs.set_fly_forward(true);
#endif
// This delay is important for the APM_RC library to work.
// We need some time for the comm between the 328 and 1280 to be established.
int old_pulse = 0;
while (millis()<=1000 && (abs(old_pulse - APM_RC.InputCh(g.flight_mode_channel)) > 5 ||
APM_RC.InputCh(g.flight_mode_channel) == 1000 ||
APM_RC.InputCh(g.flight_mode_channel) == 1200)) {
old_pulse = APM_RC.InputCh(g.flight_mode_channel);
while (millis()<=1000
&& (abs(old_pulse - hal.rcin->read(g.flight_mode_channel)) > 5
|| hal.rcin->read(g.flight_mode_channel) == 1000
|| hal.rcin->read(g.flight_mode_channel) == 1200))
{
old_pulse = hal.rcin->read(g.flight_mode_channel);
delay(25);
}
g_gps->update();
@ -324,9 +303,9 @@ static void startup_ground(void)
// 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
// ready to fly
Serial.set_blocking_writes(false);
hal.uartC->set_blocking_writes(false);
if (gcs3.initialised) {
Serial3.set_blocking_writes(false);
hal.uartC->set_blocking_writes(false);
}
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
@ -438,13 +417,13 @@ static void startup_INS_ground(bool force_accel_level)
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate,
mavlink_delay, flash_leds, &timer_scheduler);
flash_leds);
#if HIL_MODE == HIL_MODE_DISABLED
if (force_accel_level || g.manual_level == 0) {
// when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do
// it once via the ground station
ins.init_accel(mavlink_delay, flash_leds);
ins.init_accel(flash_leds);
}
#endif
ahrs.set_fly_forward(true);
@ -541,9 +520,9 @@ static void check_usb_mux(void)
// the user has switched to/from the telemetry port
usb_connected = usb_check;
if (usb_connected) {
Serial.begin(SERIAL0_BAUD);
hal.uartA->begin(SERIAL0_BAUD);
} else {
Serial.begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
}
}
#endif
@ -564,8 +543,7 @@ void flash_leds(bool on)
*/
uint16_t board_voltage(void)
{
static AP_AnalogSource_Arduino vcc(ANALOG_PIN_VCC);
return vcc.read_vcc();
return vcc_pin->read_latest();
}
@ -574,20 +552,7 @@ uint16_t board_voltage(void)
*/
static void reboot_apm(void)
{
cliSerial->printf_P(PSTR("REBOOTING\n"));
delay(100); // let serial flush
// see http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1250663814/
// for the method
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
// this relies on the bootloader resetting the watchdog, which
// APM1 doesn't do
cli();
wdt_enable(WDTO_15MS);
#else
// this works on APM1
void (*fn)(void) = NULL;
fn();
#endif
hal.scheduler->reboot();
while (1);
}
@ -630,3 +595,5 @@ static void print_comma(void)
{
cliSerial->print_P(PSTR(","));
}

60
ArduPlane/test.pde
View File

@ -134,12 +134,12 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
delay(20);
// New radio frame? (we could use also if((millis()- timer) > 20)
if (APM_RC.GetState() == 1) {
if (hal.rcin->valid() > 0) {
cliSerial->print_P(PSTR("CH:"));
for(int16_t i = 0; i < 8; i++) {
cliSerial->print(APM_RC.InputCh(i)); // Print channel values
cliSerial->print(hal.rcin->read(i)); // Print channel values
print_comma();
APM_RC.OutputCh(i, APM_RC.InputCh(i)); // Copy input to Servos
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}
@ -192,7 +192,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
{
byte fail_test;
uint8_t fail_test;
print_hit_enter();
for(int16_t i = 0; i < 50; i++) {
delay(20);
@ -319,7 +319,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
cliSerial->printf_P(PSTR("Loiter radius: %d\n\n"), (int)g.loiter_radius);
for(byte i = 0; i <= g.command_total; i++) {
for(uint8_t i = 0; i <= g.command_total; i++) {
struct Location temp = get_cmd_with_index(i);
test_wp_print(&temp, i);
}
@ -328,7 +328,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
}
static void
test_wp_print(struct Location *cmd, byte wp_index)
test_wp_print(struct Location *cmd, uint8_t wp_index)
{
cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)wp_index,
@ -349,8 +349,8 @@ test_xbee(uint8_t argc, const Menu::arg *argv)
while(1) {
if (Serial3.available())
Serial3.write(Serial3.read());
if (hal.uartC->available())
hal.uartC->write(hal.uartC->read());
if(cliSerial->available() > 0) {
return (0);
@ -371,7 +371,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
while(1) {
delay(20);
byte switchPosition = readSwitch();
uint8_t switchPosition = readSwitch();
if (oldSwitchPosition != switchPosition) {
cliSerial->printf_P(PSTR("Position %d\n"), (int)switchPosition);
oldSwitchPosition = switchPosition;
@ -389,20 +389,20 @@ static int8_t
test_logging(uint8_t argc, const Menu::arg *argv)
{
cliSerial->println_P(PSTR("Testing dataflash logging"));
if (!DataFlash.CardInserted()) {
if (!hal.dataflash->media_present()) {
cliSerial->println_P(PSTR("ERR: No dataflash inserted"));
return 0;
}
DataFlash.ReadManufacturerID();
hal.dataflash->read_mfg_id();
cliSerial->printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"),
(unsigned)DataFlash.df_manufacturer,
(unsigned)DataFlash.df_device);
cliSerial->printf_P(PSTR("NumPages: %u PageSize: %u\n"),
(unsigned)DataFlash.df_NumPages+1,
(unsigned)DataFlash.df_PageSize);
DataFlash.StartRead(DataFlash.df_NumPages+1);
(unsigned)hal.dataflash->mfg_id(),
(unsigned)hal.dataflash->device_id());
cliSerial->printf_P(PSTR("NumPages: %u\n"),
(unsigned)hal.dataflash->num_pages()+1);
hal.dataflash->start_read(hal.dataflash->num_pages()+1);
cliSerial->printf_P(PSTR("Format version: %lx Expected format version: %lx\n"),
(unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT);
(unsigned long)hal.dataflash->read_dword(),
(unsigned long)DF_LOGGING_FORMAT);
return 0;
}
@ -416,13 +416,13 @@ static int8_t
test_adc(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
adc.Init(&timer_scheduler);
adc.Init();
delay(1000);
cliSerial->printf_P(PSTR("ADC\n"));
delay(1000);
while(1) {
for (int16_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
for (int8_t i=0; i<9; i++) cliSerial->printf_P(PSTR("%.1f\t"),adc.Ch(i));
cliSerial->println();
delay(100);
if(cliSerial->available() > 0) {
@ -468,7 +468,7 @@ test_ins(uint8_t argc, const Menu::arg *argv)
//cliSerial->printf_P(PSTR("Calibrating."));
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate,
delay, flash_leds, &timer_scheduler);
flash_leds);
ahrs.reset();
print_hit_enter();
@ -531,7 +531,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// we need the AHRS initialised for this test
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate,
delay, flash_leds, &timer_scheduler);
flash_leds);
ahrs.reset();
int16_t counter = 0;
@ -603,7 +603,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
static int8_t
test_airspeed(uint8_t argc, const Menu::arg *argv)
{
float airspeed_ch = pitot_analog_source.read();
float airspeed_ch = pitot_analog_source->read_average();
// cliSerial->println(pitot_analog_source.read());
cliSerial->printf_P(PSTR("airspeed_ch: %.1f\n"), airspeed_ch);
@ -666,14 +666,16 @@ test_rawgps(uint8_t argc, const Menu::arg *argv)
delay(1000);
while(1) {
if (Serial3.available()) {
digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS
Serial1.write(Serial3.read());
// Blink Yellow LED if we are sending data to GPS
if (hal.uartC->available()) {
digitalWrite(B_LED_PIN, LED_ON);
hal.uartB->write(hal.uartC->read());
digitalWrite(B_LED_PIN, LED_OFF);
}
if (Serial1.available()) {
digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS
Serial3.write(Serial1.read());
// Blink Red LED if we are receiving data from GPS
if (hal.uartB->available()) {
digitalWrite(C_LED_PIN, LED_ON);
hal.uartC->write(hal.uartB->read());
digitalWrite(C_LED_PIN, LED_OFF);
}
if(cliSerial->available() > 0) {