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

Rover: coversion to class now complete

This commit is contained in:
Andrew Tridgell 2015-05-12 17:00:25 +10:00
parent adbf9c362e
commit b731ebfd9e
15 changed files with 458 additions and 987 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
APMrover2/APMrover2.pde
View File

@ -32,31 +32,6 @@
Please contribute your ideas! See http://dev.ardupilot.com for details
*/
// Radio setup:
// APM INPUT (Rec = receiver)
// Rec ch1: Steering
// Rec ch2: not used
// Rec ch3: Throttle
// Rec ch4: not used
// Rec ch5: not used
// Rec ch6: not used
// Rec ch7: Option channel to 2 position switch
// Rec ch8: Mode channel to 6 position switch
// APM OUTPUT
// Ch1: Wheel servo (direction)
// Ch2: not used
// Ch3: to the motor ESC
// Ch4: not used
////////////////////////////////////////////////////////////////////////////////
// Header includes
////////////////////////////////////////////////////////////////////////////////
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
// Libraries
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
@ -126,11 +101,57 @@
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include "Rover.h"
#include "utility/FastDelegate.h"
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
static Rover rover;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpmf-conversions"
#define SCHED_TASK(func) AP_HAL_CLASSPROC_VOID(&rover, &Rover::func)
/*
scheduler table - all regular tasks should be 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 Rover::scheduler_tasks[] PROGMEM = {
{ SCHED_TASK(read_radio), 1, 1000 },
{ SCHED_TASK(ahrs_update), 1, 6400 },
{ SCHED_TASK(read_sonars), 1, 2000 },
{ SCHED_TASK(update_current_mode), 1, 1500 },
{ SCHED_TASK(set_servos), 1, 1500 },
{ SCHED_TASK(update_GPS_50Hz), 1, 2500 },
{ SCHED_TASK(update_GPS_10Hz), 5, 2500 },
{ SCHED_TASK(update_alt), 5, 3400 },
{ SCHED_TASK(navigate), 5, 1600 },
{ SCHED_TASK(update_compass), 5, 2000 },
{ SCHED_TASK(update_commands), 5, 1000 },
{ SCHED_TASK(update_logging1), 5, 1000 },
{ SCHED_TASK(update_logging2), 5, 1000 },
{ SCHED_TASK(gcs_retry_deferred), 1, 1000 },
{ SCHED_TASK(gcs_update), 1, 1700 },
{ SCHED_TASK(gcs_data_stream_send), 1, 3000 },
{ SCHED_TASK(read_control_switch), 15, 1000 },
{ SCHED_TASK(read_trim_switch), 5, 1000 },
{ SCHED_TASK(read_battery), 5, 1000 },
{ SCHED_TASK(read_receiver_rssi), 5, 1000 },
{ SCHED_TASK(update_events), 1, 1000 },
{ SCHED_TASK(check_usb_mux), 15, 1000 },
{ SCHED_TASK(mount_update), 1, 600 },
{ SCHED_TASK(gcs_failsafe_check), 5, 600 },
{ SCHED_TASK(compass_accumulate), 1, 900 },
{ SCHED_TASK(update_notify), 1, 300 },
{ SCHED_TASK(one_second_loop), 50, 3000 },
#if FRSKY_TELEM_ENABLED == ENABLED
{ SCHED_TASK(frsky_telemetry_send), 10, 100 }
#endif
};
#pragma GCC diagnostic pop
/*
setup is called when the sketch starts
*/
@ -154,7 +175,7 @@ void Rover::setup()
init_ardupilot();
// initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]));
scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0]), this);
}
/*
@ -178,7 +199,6 @@ void Rover::loop()
// tell the scheduler one tick has passed
scheduler.tick();
scheduler.run(19500U);
}
@ -331,7 +351,7 @@ void Rover::one_second_loop(void)
// cope with changes to mavlink system ID
mavlink_system.sysid = g.sysid_this_mav;
uint8_t Rover::counter;
static uint8_t counter;
counter++;
@ -359,7 +379,7 @@ void Rover::one_second_loop(void)
void Rover::update_GPS_50Hz(void)
{
uint32_t Rover::last_gps_reading[GPS_MAX_INSTANCES];
static uint32_t last_gps_reading[GPS_MAX_INSTANCES];
gps.update();
for (uint8_t i=0; i<gps.num_sensors(); i++) {
@ -540,4 +560,13 @@ void Rover::update_navigation()
}
}
void setup(void)
{
rover.setup();
}
void loop(void)
{
rover.loop();
}
AP_HAL_MAIN();

245
APMrover2/GCS_Mavlink.pde
View File

@ -3,12 +3,6 @@
// default sensors are present and healthy: gyro, accelerometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
// use this to prevent recursion during sensor init
bool Rover::in_mavlink_delay;
// true if we are out of time in our event timeslice
static bool gcs_out_of_time;
// check if a message will fit in the payload space available
#define CHECK_PAYLOAD_SIZE(id) if (txspace < MAVLINK_NUM_NON_PAYLOAD_BYTES+MAVLINK_MSG_ID_## id ##_LEN) return false
@ -193,7 +187,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan)
}
void Rover:: send_location(mavlink_channel_t chan)
void Rover::send_location(mavlink_channel_t chan)
{
uint32_t fix_time;
// if we have a GPS fix, take the time as the last fix time. That
@ -220,7 +214,7 @@ void Rover:: send_location(mavlink_channel_t chan)
ahrs.yaw_sensor);
}
void Rover:: send_nav_controller_output(mavlink_channel_t chan)
void Rover::send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
@ -234,7 +228,7 @@ void Rover:: send_nav_controller_output(mavlink_channel_t chan)
nav_controller->crosstrack_error());
}
void Rover:: send_servo_out(mavlink_channel_t chan)
void Rover::send_servo_out(mavlink_channel_t chan)
{
#if HIL_MODE != HIL_MODE_DISABLED
// normalized values scaled to -10000 to 10000
@ -256,7 +250,7 @@ void Rover:: send_servo_out(mavlink_channel_t chan)
#endif
}
void Rover:: send_radio_out(mavlink_channel_t chan)
void Rover::send_radio_out(mavlink_channel_t chan)
{
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
mavlink_msg_servo_output_raw_send(
@ -287,7 +281,7 @@ void Rover:: send_radio_out(mavlink_channel_t chan)
#endif
}
void Rover:: send_vfr_hud(mavlink_channel_t chan)
void Rover::send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
@ -300,14 +294,14 @@ void Rover:: send_vfr_hud(mavlink_channel_t chan)
}
// report simulator state
void Rover:: send_simstate(mavlink_channel_t chan)
void Rover::send_simstate(mavlink_channel_t chan)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
sitl.simstate_send(chan);
#endif
}
void Rover:: send_hwstatus(mavlink_channel_t chan)
void Rover::send_hwstatus(mavlink_channel_t chan)
{
mavlink_msg_hwstatus_send(
chan,
@ -315,7 +309,7 @@ void Rover:: send_hwstatus(mavlink_channel_t chan)
hal.i2c->lockup_count());
}
void Rover:: send_rangefinder(mavlink_channel_t chan)
void Rover::send_rangefinder(mavlink_channel_t chan)
{
if (!sonar.has_data(0) && !sonar.has_data(1)) {
// no sonar to report
@ -354,12 +348,12 @@ void Rover:: send_rangefinder(mavlink_channel_t chan)
voltage);
}
void Rover:: send_current_waypoint(mavlink_channel_t chan)
void Rover::send_current_waypoint(mavlink_channel_t chan)
{
mavlink_msg_mission_current_send(chan, mission.get_current_nav_index());
}
void Rover:: send_statustext(mavlink_channel_t chan)
void Rover::send_statustext(mavlink_channel_t chan)
{
mavlink_statustext_t *s = &gcs[chan-MAVLINK_COMM_0].pending_status;
mavlink_msg_statustext_send(
@ -390,143 +384,143 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
{
uint16_t txspace = comm_get_txspace(chan);
if (telemetry_delayed(chan)) {
if (rover.telemetry_delayed(chan)) {
return false;
}
// if we don't have at least 1ms remaining before the main loop
// wants to fire then don't send a mavlink message. We want to
// prioritise the main flight control loop over communications
if (!in_mavlink_delay && scheduler.time_available_usec() < 1200) {
gcs_out_of_time = true;
if (!rover.in_mavlink_delay && rover.scheduler.time_available_usec() < 1200) {
rover.gcs_out_of_time = true;
return false;
}
switch (id) {
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis();
send_heartbeat(chan);
rover.gcs[chan-MAVLINK_COMM_0].last_heartbeat_time = hal.scheduler->millis();
rover.send_heartbeat(chan);
return true;
case MSG_EXTENDED_STATUS1:
CHECK_PAYLOAD_SIZE(SYS_STATUS);
send_extended_status1(chan);
rover.send_extended_status1(chan);
CHECK_PAYLOAD_SIZE(POWER_STATUS);
gcs[chan-MAVLINK_COMM_0].send_power_status();
rover.gcs[chan-MAVLINK_COMM_0].send_power_status();
break;
case MSG_EXTENDED_STATUS2:
CHECK_PAYLOAD_SIZE(MEMINFO);
gcs[chan-MAVLINK_COMM_0].send_meminfo();
rover.gcs[chan-MAVLINK_COMM_0].send_meminfo();
break;
case MSG_ATTITUDE:
CHECK_PAYLOAD_SIZE(ATTITUDE);
send_attitude(chan);
rover.send_attitude(chan);
break;
case MSG_LOCATION:
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
send_location(chan);
rover.send_location(chan);
break;
case MSG_LOCAL_POSITION:
CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);
send_local_position(ahrs);
send_local_position(rover.ahrs);
break;
case MSG_NAV_CONTROLLER_OUTPUT:
if (control_mode != MANUAL) {
if (rover.control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
send_nav_controller_output(chan);
rover.send_nav_controller_output(chan);
}
break;
case MSG_GPS_RAW:
CHECK_PAYLOAD_SIZE(GPS_RAW_INT);
gcs[chan-MAVLINK_COMM_0].send_gps_raw(gps);
rover.gcs[chan-MAVLINK_COMM_0].send_gps_raw(rover.gps);
break;
case MSG_SYSTEM_TIME:
CHECK_PAYLOAD_SIZE(SYSTEM_TIME);
gcs[chan-MAVLINK_COMM_0].send_system_time(gps);
rover.gcs[chan-MAVLINK_COMM_0].send_system_time(rover.gps);
break;
case MSG_SERVO_OUT:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
send_servo_out(chan);
rover.send_servo_out(chan);
break;
case MSG_RADIO_IN:
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
gcs[chan-MAVLINK_COMM_0].send_radio_in(receiver_rssi);
rover.gcs[chan-MAVLINK_COMM_0].send_radio_in(rover.receiver_rssi);
break;
case MSG_RADIO_OUT:
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
send_radio_out(chan);
rover.send_radio_out(chan);
break;
case MSG_VFR_HUD:
CHECK_PAYLOAD_SIZE(VFR_HUD);
send_vfr_hud(chan);
rover.send_vfr_hud(chan);
break;
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
gcs[chan-MAVLINK_COMM_0].send_raw_imu(ins, compass);
rover.gcs[chan-MAVLINK_COMM_0].send_raw_imu(rover.ins, rover.compass);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(ins, compass, barometer);
rover.gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(rover.ins, rover.compass, rover.barometer);
break;
case MSG_CURRENT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_CURRENT);
send_current_waypoint(chan);
rover.send_current_waypoint(chan);
break;
case MSG_NEXT_PARAM:
CHECK_PAYLOAD_SIZE(PARAM_VALUE);
gcs[chan-MAVLINK_COMM_0].queued_param_send();
rover.gcs[chan-MAVLINK_COMM_0].queued_param_send();
break;
case MSG_NEXT_WAYPOINT:
CHECK_PAYLOAD_SIZE(MISSION_REQUEST);
gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
rover.gcs[chan-MAVLINK_COMM_0].queued_waypoint_send();
break;
case MSG_STATUSTEXT:
CHECK_PAYLOAD_SIZE(STATUSTEXT);
send_statustext(chan);
rover.send_statustext(chan);
break;
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
gcs[chan-MAVLINK_COMM_0].send_ahrs(ahrs);
rover.gcs[chan-MAVLINK_COMM_0].send_ahrs(rover.ahrs);
break;
case MSG_SIMSTATE:
CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate(chan);
rover.send_simstate(chan);
break;
case MSG_HWSTATUS:
CHECK_PAYLOAD_SIZE(HWSTATUS);
send_hwstatus(chan);
rover.send_hwstatus(chan);
break;
case MSG_RANGEFINDER:
CHECK_PAYLOAD_SIZE(RANGEFINDER);
send_rangefinder(chan);
rover.send_rangefinder(chan);
break;
case MSG_MOUNT_STATUS:
#if MOUNT == ENABLED
CHECK_PAYLOAD_SIZE(MOUNT_STATUS);
camera_mount.status_msg(chan);
rover.camera_mount.status_msg(chan);
#endif // MOUNT == ENABLED
break;
@ -539,20 +533,20 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_BATTERY2:
CHECK_PAYLOAD_SIZE(BATTERY2);
gcs[chan-MAVLINK_COMM_0].send_battery2(battery);
rover.gcs[chan-MAVLINK_COMM_0].send_battery2(rover.battery);
break;
case MSG_CAMERA_FEEDBACK:
#if CAMERA == ENABLED
CHECK_PAYLOAD_SIZE(CAMERA_FEEDBACK);
camera.send_feedback(chan, gps, ahrs, current_loc);
rover.camera.send_feedback(chan, rover.gps, rover.ahrs, rover.current_loc);
#endif
break;
case MSG_EKF_STATUS_REPORT:
#if AP_AHRS_NAVEKF_AVAILABLE
CHECK_PAYLOAD_SIZE(EKF_STATUS_REPORT);
ahrs.get_NavEKF().send_status_report(chan);
rover.ahrs.get_NavEKF().send_status_report(chan);
#endif
break;
@ -691,10 +685,10 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
void
GCS_MAVLINK::data_stream_send(void)
{
gcs_out_of_time = false;
rover.gcs_out_of_time = false;
if (!in_mavlink_delay) {
handle_log_send(DataFlash);
if (!rover.in_mavlink_delay) {
handle_log_send(rover.DataFlash);
}
if (_queued_parameter != NULL) {
@ -706,9 +700,9 @@ GCS_MAVLINK::data_stream_send(void)
}
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (in_mavlink_delay) {
if (rover.in_mavlink_delay) {
#if HIL_MODE != HIL_MODE_DISABLED
// in HIL we need to keep sending servo values to ensure
// the simulator doesn't pause, otherwise our sensor
@ -724,14 +718,14 @@ GCS_MAVLINK::data_stream_send(void)
return;
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_RAW_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU3);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_EXTENDED_STATUS1);
@ -741,7 +735,7 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_NAV_CONTROLLER_OUTPUT);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
@ -749,33 +743,33 @@ GCS_MAVLINK::data_stream_send(void)
send_message(MSG_LOCAL_POSITION);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
}
if (gcs_out_of_time) return;
if (rover.gcs_out_of_time) return;
if (stream_trigger(STREAM_EXTRA3)) {
send_message(MSG_AHRS);
@ -792,13 +786,13 @@ GCS_MAVLINK::data_stream_send(void)
void GCS_MAVLINK::handle_guided_request(AP_Mission::Mission_Command &cmd)
{
guided_WP = cmd.content.location;
rover.guided_WP = cmd.content.location;
set_mode(GUIDED);
rover.set_mode(GUIDED);
// make any new wp uploaded instant (in case we are already in Guided mode)
rtl_complete = false;
set_guided_WP();
rover.rtl_complete = false;
rover.set_guided_WP();
}
void GCS_MAVLINK::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
@ -830,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch(packet.command) {
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
set_mode(RTL);
rover.set_mode(RTL);
result = MAV_RESULT_ACCEPTED;
break;
@ -843,48 +837,48 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
roi_loc.alt = (int32_t)(packet.param7 * 100.0f);
if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) {
// switch off the camera tracking if enabled
if (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
camera_mount.set_mode_to_default();
if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) {
rover.camera_mount.set_mode_to_default();
}
} else {
// send the command to the camera mount
camera_mount.set_roi_target(roi_loc);
rover.camera_mount.set_roi_target(roi_loc);
}
result = MAV_RESULT_ACCEPTED;
break;
#endif
case MAV_CMD_MISSION_START:
set_mode(AUTO);
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_CALIBRATION:
if (is_equal(packet.param1,1.0f)) {
ins.init_gyro();
if (ins.gyro_calibrated_ok_all()) {
ahrs.reset_gyro_drift();
rover.ins.init_gyro();
if (rover.ins.gyro_calibrated_ok_all()) {
rover.ahrs.reset_gyro_drift();
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (is_equal(packet.param3,1.0f)) {
init_barometer();
rover.init_barometer();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param4,1.0f)) {
trim_radio();
rover.trim_radio();
result = MAV_RESULT_ACCEPTED;
} else if (is_equal(packet.param5,1.0f)) {
float trim_roll, trim_pitch;
AP_InertialSensor_UserInteract_MAVLink interact(this);
if (g.skip_gyro_cal) {
if (rover.g.skip_gyro_cal) {
// start with gyro calibration, otherwise if the user
// has SKIP_GYRO_CAL=1 they don't get to do it
ins.init_gyro();
rover.ins.init_gyro();
}
if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
if(rover.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
@ -908,12 +902,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
if (is_equal(packet.param1,2.0f)) {
// save first compass's offsets
compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4);
rover.compass.set_and_save_offsets(0, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
if (is_equal(packet.param1,5.0f)) {
// save secondary compass's offsets
compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4);
rover.compass.set_and_save_offsets(1, packet.param2, packet.param3, packet.param4);
result = MAV_RESULT_ACCEPTED;
}
break;
@ -922,19 +916,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
switch ((uint16_t)packet.param1) {
case MAV_MODE_MANUAL_ARMED:
case MAV_MODE_MANUAL_DISARMED:
set_mode(MANUAL);
rover.set_mode(MANUAL);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_AUTO_ARMED:
case MAV_MODE_AUTO_DISARMED:
set_mode(AUTO);
rover.set_mode(AUTO);
result = MAV_RESULT_ACCEPTED;
break;
case MAV_MODE_STABILIZE_DISARMED:
case MAV_MODE_STABILIZE_ARMED:
set_mode(LEARNING);
rover.set_mode(LEARNING);
result = MAV_RESULT_ACCEPTED;
break;
@ -944,25 +938,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
if (ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_SERVO:
if (ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_SET_RELAY:
if (ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) {
result = MAV_RESULT_ACCEPTED;
}
break;
case MAV_CMD_DO_REPEAT_RELAY:
if (ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) {
result = MAV_RESULT_ACCEPTED;
}
break;
@ -977,7 +971,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: {
if (is_equal(packet.param1,1.0f)) {
gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
result = MAV_RESULT_ACCEPTED;
}
break;
@ -999,13 +993,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_SET_MODE:
{
handle_set_mode(msg, mavlink_set_mode);
handle_set_mode(msg, AP_HAL_CLASSPROC(&rover, &Rover::mavlink_set_mode));
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
{
handle_mission_request_list(mission, msg);
handle_mission_request_list(rover.mission, msg);
break;
}
@ -1013,7 +1007,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// XXX read a WP from EEPROM and send it to the GCS
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
handle_mission_request(mission, msg);
handle_mission_request(rover.mission, msg);
break;
}
@ -1044,33 +1038,33 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
{
handle_mission_clear_all(mission, msg);
handle_mission_clear_all(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
handle_mission_set_current(mission, msg);
handle_mission_set_current(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT:
{
handle_mission_count(mission, msg);
handle_mission_count(rover.mission, msg);
break;
}
case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
{
handle_mission_write_partial_list(mission, msg);
handle_mission_write_partial_list(rover.mission, msg);
break;
}
// XXX receive a WP from GCS and store in EEPROM
case MAVLINK_MSG_ID_MISSION_ITEM:
{
if (handle_mission_item(msg, mission)) {
Log_Write_EntireMission();
if (handle_mission_item(msg, rover.mission)) {
rover.Log_Write_EntireMission();
}
break;
}
@ -1078,7 +1072,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_PARAM_SET:
{
handle_param_set(msg, &DataFlash);
handle_param_set(msg, &rover.DataFlash);
break;
}
@ -1087,7 +1081,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// allow override of RC channel values for HIL
// or for complete GCS control of switch position
// and RC PWM values.
if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs
if(msg->sysid != rover.g.sysid_my_gcs) break; // Only accept control from our gcs
mavlink_rc_channels_override_t packet;
int16_t v[8];
mavlink_msg_rc_channels_override_decode(msg, &packet);
@ -1103,17 +1097,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
hal.rcin->set_overrides(v, 8);
failsafe.rc_override_timer = millis();
failsafe_trigger(FAILSAFE_EVENT_RC, false);
rover.failsafe.rc_override_timer = hal.scheduler->millis();
rover.failsafe_trigger(FAILSAFE_EVENT_RC, false);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != g.sysid_my_gcs) break;
last_heartbeat_ms = failsafe.rc_override_timer = millis();
failsafe_trigger(FAILSAFE_EVENT_GCS, false);
if(msg->sysid != rover.g.sysid_my_gcs) break;
rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = hal.scheduler->millis();
rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false);
break;
}
@ -1164,8 +1158,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_DIGICAM_CONTROL:
{
camera.control_msg(msg);
log_picture();
rover.camera.control_msg(msg);
rover.log_picture();
break;
}
#endif // CAMERA == ENABLED
@ -1173,13 +1167,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#if MOUNT == ENABLED
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
{
camera_mount.configure_msg(msg);
rover.camera_mount.configure_msg(msg);
break;
}
case MAVLINK_MSG_ID_MOUNT_CONTROL:
{
camera_mount.control_msg(msg);
rover.camera_mount.control_msg(msg);
break;
}
#endif // MOUNT == ENABLED
@ -1187,39 +1181,39 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
case MAVLINK_MSG_ID_RADIO:
case MAVLINK_MSG_ID_RADIO_STATUS:
{
handle_radio_status(msg, DataFlash, should_log(MASK_LOG_PM));
handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM));
break;
}
case MAVLINK_MSG_ID_LOG_REQUEST_DATA:
case MAVLINK_MSG_ID_LOG_ERASE:
in_log_download = true;
rover.in_log_download = true;
// fallthru
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
if (!in_mavlink_delay) {
handle_log_message(msg, DataFlash);
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
case MAVLINK_MSG_ID_LOG_REQUEST_END:
in_log_download = false;
if (!in_mavlink_delay) {
handle_log_message(msg, DataFlash);
rover.in_log_download = false;
if (!rover.in_mavlink_delay) {
handle_log_message(msg, rover.DataFlash);
}
break;
#if HAL_CPU_CLASS > HAL_CPU_CLASS_16
case MAVLINK_MSG_ID_SERIAL_CONTROL:
handle_serial_control(msg, gps);
handle_serial_control(msg, rover.gps);
break;
case MAVLINK_MSG_ID_GPS_INJECT_DATA:
handle_gps_inject(msg, gps);
handle_gps_inject(msg, rover.gps);
break;
#endif
case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST:
gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
rover.gcs[chan-MAVLINK_COMM_0].send_autopilot_version();
break;
} // end switch
@ -1233,7 +1227,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
*/
void Rover::mavlink_delay_cb()
{
uint32_t Rover::last_1hz, last_50hz, last_5s;
static uint32_t last_1hz, last_50hz, last_5s;
if (!gcs[0].initialised || in_mavlink_delay) return;
in_mavlink_delay = true;
@ -1259,6 +1253,11 @@ void Rover::mavlink_delay_cb()
in_mavlink_delay = false;
}
void mavlink_delay_cb_static()
{
rover.mavlink_delay_cb();
}
/*
* send a message on both GCS links
*/
@ -1291,7 +1290,7 @@ void Rover::gcs_update(void)
for (uint8_t i=0; i<num_gcs; i++) {
if (gcs[i].initialised) {
#if CLI_ENABLED == ENABLED
gcs[i].update(g.cli_enabled==1?run_cli:NULL);
gcs[i].update(g.cli_enabled==1?AP_HAL_MEMBERPROC(&Rover::run_cli):NULL);
#else
gcs[i].update(NULL);
#endif
@ -1316,7 +1315,7 @@ void Rover::gcs_send_text_P(gcs_severity severity, const prog_char_t *str)
* only one fits in the queue, so if you send more than one before the
* last one gets into the serial buffer then the old one will be lost
*/
void gcs_send_text_fmt(const prog_char_t *fmt, ...)
void Rover::gcs_send_text_fmt(const prog_char_t *fmt, ...)
{
va_list arg_list;
gcs[0].pending_status.severity = (uint8_t)SEVERITY_LOW;

48
APMrover2/Log.pde
View File

@ -4,31 +4,23 @@
#if CLI_ENABLED == ENABLED
// Code to Write and Read packets from DataFlash log memory
// Code to interact with the user to dump or erase logs
// These are function definitions so the Menu can be constructed before the functions
// are defined below. Order matters to the compiler.
static int8_t dump_log(uint8_t argc, const Menu::arg *argv);
static int8_t erase_logs(uint8_t argc, const Menu::arg *argv);
static int8_t select_logs(uint8_t argc, const Menu::arg *argv);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Coommon for implementation details
static const struct Menu::command log_menu_commands[] PROGMEM = {
{"dump", dump_log},
{"erase", erase_logs},
{"enable", select_logs},
{"disable", select_logs}
{"dump", MENU_FUNC(dump_log)},
{"erase", MENU_FUNC(erase_logs)},
{"enable", MENU_FUNC(select_logs)},
{"disable", MENU_FUNC(select_logs)}
};
// A Macro to create the Menu
MENU2(log_menu, "Log", log_menu_commands, print_log_menu);
MENU2(log_menu, "Log", log_menu_commands, MENU_FUNC(print_log_menu));
static bool
print_log_menu(void)
bool Rover::print_log_menu(void)
{
cliSerial->printf_P(PSTR("logs enabled: "));
@ -63,40 +55,38 @@ print_log_menu(void)
return(true);
}
static int8_t
dump_log(uint8_t argc, const Menu::arg *argv)
int8_t Rover::dump_log(uint8_t argc, const Menu::arg *argv)
{
int16_t dump_log;
int16_t dump_log_num;
uint16_t dump_log_start;
uint16_t dump_log_end;
uint16_t last_log_num;
// check that the requested log number can be read
dump_log = argv[1].i;
dump_log_num = argv[1].i;
last_log_num = DataFlash.find_last_log();
if (dump_log == -2) {
if (dump_log_num == -2) {
DataFlash.DumpPageInfo(cliSerial);
return(-1);
} else if (dump_log <= 0) {
} else if (dump_log_num <= 0) {
cliSerial->printf_P(PSTR("dumping all\n"));
Log_Read(0, 1, 0);
return(-1);
} else if ((argc != 2)
|| ((uint16_t)dump_log > last_log_num))
|| ((uint16_t)dump_log_num > last_log_num))
{
cliSerial->printf_P(PSTR("bad log number\n"));
return(-1);
}
DataFlash.get_log_boundaries(dump_log, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log, dump_log_start, dump_log_end);
DataFlash.get_log_boundaries(dump_log_num, dump_log_start, dump_log_end);
Log_Read((uint16_t)dump_log_num, dump_log_start, dump_log_end);
return 0;
}
static int8_t
erase_logs(uint8_t argc, const Menu::arg *argv)
int8_t Rover::erase_logs(uint8_t argc, const Menu::arg *argv)
{
in_mavlink_delay = true;
do_erase_logs();
@ -104,8 +94,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv)
return 0;
}
static int8_t
select_logs(uint8_t argc, const Menu::arg *argv)
int8_t Rover::select_logs(uint8_t argc, const Menu::arg *argv)
{
uint16_t bits;
@ -151,8 +140,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
return(0);
}
static int8_t
process_logs(uint8_t argc, const Menu::arg *argv)
int8_t Rover::process_logs(uint8_t argc, const Menu::arg *argv)
{
log_menu.run();
return 0;
@ -410,7 +398,7 @@ void Rover::Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page)
cliSerial->println_P(PSTR(HAL_BOARD_NAME));
DataFlash.LogReadProcess(log_num, start_page, end_page,
print_mode,
AP_HAL_MEMBERPROC(&Rover::print_mode),
cliSerial);
}
#endif // CLI_ENABLED

10
APMrover2/Parameters.pde
View File

@ -4,12 +4,12 @@
APMRover2 parameter definitions
*/
#define GSCALAR(v, name, def) { g.v.vtype, name, Parameters::k_param_ ## v, &g.v, {def_value:def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &g.v, {group_info:class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &v, {group_info:class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &v, {group_info : class::var_info} }
#define GSCALAR(v, name, def) { rover.g.v.vtype, name, Parameters::k_param_ ## v, &rover.g.v, {def_value:def} }
#define GGROUP(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &rover.g.v, {group_info:class::var_info} }
#define GOBJECT(v, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## v, &rover.v, {group_info:class::var_info} }
#define GOBJECTN(v, pname, name, class) { AP_PARAM_GROUP, name, Parameters::k_param_ ## pname, &rover.v, {group_info : class::var_info} }
const AP_Param::Info var_info[] PROGMEM = {
const AP_Param::Info Rover::var_info[] PROGMEM = {
GSCALAR(format_version, "FORMAT_VERSION", 1),
GSCALAR(software_type, "SYSID_SW_TYPE", Parameters::k_software_type),

90
APMrover2/Rover.cpp
View File

@ -1,90 +0,0 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
main Rover class, containing all vehicle specific state
*/
/*
scheduler table - all regular tasks should be 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 Rover::scheduler_tasks[] PROGMEM = {
{ read_radio, 1, 1000 },
{ ahrs_update, 1, 6400 },
{ read_sonars, 1, 2000 },
{ update_current_mode, 1, 1500 },
{ set_servos, 1, 1500 },
{ update_GPS_50Hz, 1, 2500 },
{ update_GPS_10Hz, 5, 2500 },
{ update_alt, 5, 3400 },
{ navigate, 5, 1600 },
{ update_compass, 5, 2000 },
{ update_commands, 5, 1000 },
{ update_logging1, 5, 1000 },
{ update_logging2, 5, 1000 },
{ gcs_retry_deferred, 1, 1000 },
{ gcs_update, 1, 1700 },
{ gcs_data_stream_send, 1, 3000 },
{ read_control_switch, 15, 1000 },
{ read_trim_switch, 5, 1000 },
{ read_battery, 5, 1000 },
{ read_receiver_rssi, 5, 1000 },
{ update_events, 1, 1000 },
{ check_usb_mux, 15, 1000 },
{ mount_update, 1, 600 },
{ gcs_failsafe_check, 5, 600 },
{ compass_accumulate, 1, 900 },
{ update_notify, 1, 300 },
{ one_second_loop, 50, 3000 },
#if FRSKY_TELEM_ENABLED == ENABLED
{ frsky_telemetry_send, 10, 100 }
#endif
};
Rover::Rover(void) :
param_loader(var_info)
channel_steer(NULL),
channel_throttle(NULL),
channel_learn(NULL),
in_log_download(false),
modes(&g.mode1),
#if AP_AHRS_NAVEKF_AVAILABLE
ahrs(ins, barometer, gps, sonar),
#else
ahrs(ins, barometer, gps),
#endif
L1_controller(ahrs),
nav_controller(&L1_controller),
steerController(ahrs),
mission(ahrs, &start_command, &verify_command, &exit_mission),
ServoRelayEvents(relay),
#if CAMERA == ENABLED
AP_camera(&relay),
#endif
#if MOUNT == ENABLED
camera_mount(ahrs, current_loc),
#endif
control_mode(INITIALISING),
ground_start_count(20),
throttle(500),
frsky_telemetry(ahrs, battery),
home(ahrs.get_home()),
G_Dt(0.02),
{
}

155
APMrover2/Rover.h
View File

@ -17,9 +17,85 @@
main Rover class, containing all vehicle specific state
*/
#include <math.h>
#include <stdarg.h>
// Libraries
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_Menu.h>
#include <AP_Param.h>
#include <StorageManager.h>
#include <AP_GPS.h> // ArduPilot GPS library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
#include <AP_ADC_AnalogSource.h>
#include <AP_Baro.h>
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_AHRS.h> // ArduPilot Mega DCM Library
#include <AP_NavEKF.h>
#include <AP_Mission.h> // Mission command library
#include <AP_Rally.h>
#include <AP_Terrain.h>
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <Filter.h> // Filter library
#include <Butter.h> // Filter library - butterworth filter
#include <AP_Buffer.h> // FIFO buffer library
#include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // Mode Filter from Filter library
#include <AP_Relay.h> // APM relay
#include <AP_ServoRelayEvents.h>
#include <AP_Mount.h> // Camera/Antenna mount
#include <AP_Camera.h> // Camera triggering
#include <GCS_MAVLink.h> // MAVLink GCS definitions
#include <AP_SerialManager.h> // Serial manager library
#include <AP_Airspeed.h> // needed for AHRS build
#include <AP_Vehicle.h> // needed for AHRS build
#include <DataFlash.h>
#include <AP_RCMapper.h> // RC input mapping library
#include <SITL.h>
#include <AP_Scheduler.h> // main loop scheduler
#include <stdarg.h>
#include <AP_Navigation.h>
#include <APM_Control.h>
#include <AP_L1_Control.h>
#include <AP_BoardConfig.h>
#include <AP_Frsky_Telem.h>
#include <AP_HAL_AVR.h>
#include <AP_HAL_SITL.h>
#include <AP_HAL_PX4.h>
#include <AP_HAL_VRBRAIN.h>
#include <AP_HAL_FLYMAPLE.h>
#include <AP_HAL_Linux.h>
#include <AP_HAL_Empty.h>
#include "compat.h"
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#include <AP_OpticalFlow.h> // Optical Flow library
// Configuration
#include "config.h"
// Local modules
#include "defines.h"
#include "Parameters.h"
#include "GCS.h"
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
class Rover {
public:
friend class GCS_MAVLINK;
friend class Parameters;
Rover(void);
// public member functions
void setup(void);
void loop(void);
@ -33,7 +109,7 @@ private:
AP_Param param_loader;
// the rate we run the main loop at
const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor::RATE_50HZ;
const AP_InertialSensor::Sample_rate ins_sample_rate;
// all settable parameters
Parameters g;
@ -58,7 +134,7 @@ private:
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
DataFlash_APM2 DataFlash;
#elif defined(HAL_BOARD_LOG_DIRECTORY)
DataFlash_File DataFlash(HAL_BOARD_LOG_DIRECTORY);
DataFlash_File DataFlash;
#else
DataFlash_Empty DataFlash;
#endif
@ -104,7 +180,7 @@ private:
// GCS handling
AP_SerialManager serial_manager;
const uint8_t num_gcs = MAVLINK_COMM_NUM_BUFFERS;
const uint8_t num_gcs;
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
@ -153,7 +229,7 @@ private:
// A tracking variable for type of failsafe active
// Used for failsafe based on loss of RC signal or GCS signal. See
// FAILSAFE_EVENT_*
static struct {
struct {
uint8_t bits;
uint32_t rc_override_timer;
uint32_t start_time;
@ -169,7 +245,7 @@ private:
uint8_t ground_start_count;
// Location & Navigation
const float radius_of_earth = 6378100; // meters
const float radius_of_earth; // meters
// true if we have a position estimate from AHRS
bool have_position;
@ -282,6 +358,14 @@ private:
static const AP_Scheduler::Task scheduler_tasks[];
// 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;
static const AP_Param::Info var_info[];
private:
// private member functions
void ahrs_update();
@ -312,13 +396,11 @@ private:
void send_current_waypoint(mavlink_channel_t chan);
void send_statustext(mavlink_channel_t chan);
bool telemetry_delayed(mavlink_channel_t chan);
void mavlink_delay_cb();
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_retry_deferred(void);
bool print_log_menu(void);
void do_erase_logs(void);
void Log_Write_Performance();
void Log_Write_Steering();
@ -326,23 +408,13 @@ private:
void Log_Write_EntireMission();
void Log_Write_Control_Tuning();
void Log_Write_Nav_Tuning();
void Log_Write_Attitude();
void Log_Write_Sonar();
void Log_Write_Current();
void Log_Write_Attitude();
void Log_Write_RC(void);
void Log_Write_Baro(void);
void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);
void start_logging() ;
void Log_Write_Startup(uint8_t type);
void Log_Write_EntireMission();
void Log_Write_Current();
void Log_Write_Nav_Tuning();
void Log_Write_Performance();
void Log_Write_Control_Tuning();
void Log_Write_Sonar();
void Log_Write_Attitude();
void start_logging();
void Log_Write_RC(void);
void load_parameters(void);
void throttle_slew_limit(int16_t last_throttle);
bool auto_check_trigger(void);
@ -372,7 +444,6 @@ private:
void reset_control_switch();
void read_trim_switch();
void update_events(void);
void failsafe_check();
void navigate();
void reached_waypoint();
void set_control_channels(void);
@ -416,4 +487,50 @@ private:
bool should_log(uint32_t mask);
void frsky_telemetry_send(void);
void print_hit_enter();
void gcs_send_text_fmt(const prog_char_t *fmt, ...);
void Log_Write_Cmd(const AP_Mission::Mission_Command &cmd);
void print_mode(AP_HAL::BetterStream *port, uint8_t mode);
bool start_command(const AP_Mission::Mission_Command& cmd);
bool verify_command(const AP_Mission::Mission_Command& cmd);
void do_nav_wp(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_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);
public:
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_erase(uint8_t argc, const Menu::arg *argv);
int8_t setup_mode(uint8_t argc, const Menu::arg *argv);
int8_t reboot_board(uint8_t, const Menu::arg*);
int8_t main_menu_help(uint8_t argc, const Menu::arg *argv);
int8_t test_mode(uint8_t argc, const Menu::arg *argv);
void run_cli(AP_HAL::UARTDriver *port);
void mavlink_delay_cb();
void failsafe_check();
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_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_sonar(uint8_t argc, const Menu::arg *argv);
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
int8_t test_shell(uint8_t argc, const Menu::arg *argv);
#endif
};
#define MENU_FUNC(func) AP_HAL_CLASSPROC(&rover, &Rover::func)

61
APMrover2/Rover.pde Normal file
View File

@ -0,0 +1,61 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
main Rover class, containing all vehicle specific state
*/
Rover::Rover(void) :
param_loader(var_info),
ins_sample_rate(AP_InertialSensor::RATE_50HZ),
channel_steer(NULL),
channel_throttle(NULL),
channel_learn(NULL),
in_log_download(false),
#if defined(HAL_BOARD_LOG_DIRECTORY)
DataFlash(HAL_BOARD_LOG_DIRECTORY),
#endif
modes(&g.mode1),
#if AP_AHRS_NAVEKF_AVAILABLE
ahrs(ins, barometer, gps, sonar),
#else
ahrs(ins, barometer, gps),
#endif
L1_controller(ahrs),
nav_controller(&L1_controller),
steerController(ahrs),
mission(ahrs,
AP_HAL_MEMBERPROC(&Rover::start_command),
AP_HAL_MEMBERPROC(&Rover::verify_command),
AP_HAL_MEMBERPROC(&Rover::exit_mission)),
ServoRelayEvents(relay),
num_gcs(MAVLINK_COMM_NUM_BUFFERS),
#if CAMERA == ENABLED
camera(&relay),
#endif
#if MOUNT == ENABLED
camera_mount(ahrs, current_loc),
#endif
control_mode(INITIALISING),
ground_start_count(20),
throttle(500),
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry(ahrs, battery),
#endif
home(ahrs.get_home()),
G_Dt(0.02),
radius_of_earth(6378100)
{
}

2
APMrover2/commands.pde
View File

@ -54,7 +54,7 @@ void Rover::set_guided_WP(void)
// run this at setup on the ground
// -------------------------------
void init_home()
void Rover::init_home()
{
if (!have_position) {
// we need position information

15
APMrover2/commands_logic.pde
View File

@ -1,22 +1,9 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// forward declarations to make compiler happy
void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd);
void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd);
void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd);
void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd);
void Rover::do_set_home(const AP_Mission::Mission_Command& cmd);
bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd);
#if CAMERA == ENABLED
void Rover::do_digicam_configure(const AP_Mission::Mission_Command& cmd);
void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd);
#endif
/********************************************************************************/
// Command Event Handlers
/********************************************************************************/
static bool
start_command(const AP_Mission::Mission_Command& cmd)
bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
{
// log when new commands start
if (should_log(MASK_LOG_CMD)) {

3
APMrover2/compat.h
View File

@ -5,8 +5,5 @@
#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__

9
APMrover2/failsafe.pde
View File

@ -16,8 +16,8 @@
void Rover::failsafe_check()
{
static uint16_t last_mainLoop_count;
uint32_t Rover::last_timestamp;
bool Rover::in_failsafe;
static uint32_t last_timestamp;
static bool in_failsafe;
uint32_t tnow = hal.scheduler->micros();
if (mainLoop_count != last_mainLoop_count) {
@ -48,3 +48,8 @@ void Rover::failsafe_check()
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
}
}
static void failsafe_check_static()
{
rover.failsafe_check();
}

2
APMrover2/sensors.pde
View File

@ -21,7 +21,7 @@ void Rover::read_battery(void)
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void read_receiver_rssi(void)
void Rover::read_receiver_rssi(void)
{
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->voltage_average() * 50;

592
APMrover2/setup.pde
View File

@ -2,44 +2,18 @@
#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_flightmodes (uint8_t argc, const Menu::arg *argv);
#if !defined( __AVR_ATmega1280__ )
int8_t Rover:: setup_accel_scale (uint8_t argc, const Menu::arg *argv);
int8_t Rover:: setup_set (uint8_t argc, const Menu::arg *argv);
#endif
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (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},
{"modes", setup_flightmodes},
#if !defined( __AVR_ATmega1280__ )
{"accel", setup_accel_scale},
#endif
{"compass", setup_compass},
{"declination", setup_declination},
{"show", setup_show},
#if !defined( __AVR_ATmega1280__ )
{"set", setup_set},
#endif
{"erase", setup_erase},
{"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.
static int8_t
setup_mode(uint8_t argc, const Menu::arg *argv)
int8_t Rover::setup_mode(uint8_t argc, const Menu::arg *argv)
{
// Give the user some guidance
cliSerial->printf_P(PSTR("Setup Mode\n"
@ -54,331 +28,7 @@ setup_mode(uint8_t argc, const Menu::arg *argv)
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)
{
#if !defined( __AVR_ATmega1280__ )
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;
}
//Print differently for different types, and include parameter type in output.
switch (type) {
case AP_PARAM_INT8:
cliSerial->printf_P(PSTR("INT8 %s: %d\n"), argv[1].str, (int)((AP_Int8 *)param)->get());
break;
case AP_PARAM_INT16:
cliSerial->printf_P(PSTR("INT16 %s: %d\n"), argv[1].str, (int)((AP_Int16 *)param)->get());
break;
case AP_PARAM_INT32:
cliSerial->printf_P(PSTR("INT32 %s: %ld\n"), argv[1].str, (long)((AP_Int32 *)param)->get());
break;
case AP_PARAM_FLOAT:
cliSerial->printf_P(PSTR("FLOAT %s: %f\n"), argv[1].str, (double)((AP_Float *)param)->get());
break;
default:
cliSerial->printf_P(PSTR("Unhandled parameter type for %s: %d.\n"), argv[1].str, type);
break;
}
return 0;
}
#endif
// clear the area
print_blanks(8);
report_radio();
report_batt_monitor();
report_gains();
report_throttle();
report_modes();
report_compass();
cliSerial->printf_P(PSTR("Raw Values\n"));
print_divider();
AP_Param::show_all(cliSerial);
return(0);
}
#if !defined( __AVR_ATmega1280__ )
//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
int8_t Rover::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;
}
#endif
// 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"));
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++){
delay(20);
read_radio();
}
if(channel_steer->radio_in < 500){
while(1){
cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000);
// stop here
}
}
channel_steer->radio_min = channel_steer->radio_in;
channel_throttle->radio_min = channel_throttle->radio_in;
g.rc_2.radio_min = g.rc_2.radio_in;
g.rc_4.radio_min = g.rc_4.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_steer->radio_max = channel_steer->radio_in;
channel_throttle->radio_max = channel_throttle->radio_in;
g.rc_2.radio_max = g.rc_2.radio_in;
g.rc_4.radio_max = g.rc_4.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_steer->radio_trim = channel_steer->radio_in;
g.rc_2.radio_trim = 1500;
g.rc_4.radio_trim = 1500;
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){
delay(20);
// Filters radio input - adjust filters in the radio.pde file
// ----------------------------------------------------------
read_radio();
channel_steer->update_min_max();
channel_throttle->update_min_max();
g.rc_2.update_min_max();
g.rc_4.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_steer->save_eeprom();
channel_throttle->save_eeprom();
g.rc_2.save_eeprom();
g.rc_4.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_flightmodes(uint8_t argc, const Menu::arg *argv)
{
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();
trim_radio();
while(1){
delay(20);
read_radio();
switchPosition = readSwitch();
// look for control switch change
if (oldSwitchPosition != switchPosition){
// force position 5 to MANUAL
if (switchPosition > 4) {
modes[switchPosition] = MANUAL;
}
// update our current mode
mode = modes[switchPosition];
// update the user
print_switch(switchPosition, mode);
// Remember switch position
oldSwitchPosition = switchPosition;
}
// look for stick input
int radioInputSwitch = radio_input_switch();
if (radioInputSwitch != 0){
mode += radioInputSwitch;
while (
mode != MANUAL &&
mode != HOLD &&
mode != LEARNING &&
mode != STEERING &&
mode != AUTO &&
mode != RTL)
{
if (mode > RTL)
mode = MANUAL;
else
mode += radioInputSwitch;
}
// Override position 5
if(switchPosition > 4)
mode = MANUAL;
// save new mode
modes[switchPosition] = mode;
// print new mode
print_switch(switchPosition, mode);
}
// escape hatch
if(cliSerial->available() > 0){
// save changes
for (mode=0; mode<6; mode++)
modes[mode].save();
report_modes();
print_done();
return (0);
}
}
}
static int8_t
setup_declination(uint8_t argc, const Menu::arg *argv)
{
compass.set_declination(radians(argv[1].f));
report_compass();
return 0;
}
static int8_t
setup_erase(uint8_t argc, const Menu::arg *argv)
int8_t Rover::setup_erase(uint8_t argc, const Menu::arg *argv)
{
int c;
@ -394,233 +44,6 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
return 0;
}
/*
handle full accelerometer calibration via user dialog
*/
#if !defined( __AVR_ATmega1280__ )
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();
ins.init(AP_InertialSensor::COLD_START,
ins_sample_rate);
AP_InertialSensor_UserInteractStream interact(hal.console);
if(ins.calibrate_accel(&interact, trim_roll, trim_pitch)) {
// reset ahrs's trim to suggested values from calibration routine
ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0));
}
return(0);
}
#endif
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
/***************************************************************************/
void Rover::report_batt_monitor()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Batt Mointor\n"));
print_divider();
if (battery.num_instances() == 0) {
cliSerial->printf_P(PSTR("Batt monitoring disabled"));
} else if (!battery.has_current()) {
cliSerial->printf_P(PSTR("Monitoring batt volts"));
} else {
cliSerial->printf_P(PSTR("Monitoring volts and current"));
}
print_blanks(2);
}
void Rover::report_radio()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Radio\n"));
print_divider();
// radio
print_radio_values();
print_blanks(2);
}
void Rover::report_gains()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Gains\n"));
print_divider();
cliSerial->printf_P(PSTR("speed throttle:\n"));
print_PID(&g.pidSpeedThrottle);
print_blanks(2);
}
void Rover::report_throttle()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Throttle\n"));
print_divider();
cliSerial->printf_P(PSTR("min: %u\n"
"max: %u\n"
"cruise: %u\n"
"failsafe_enabled: %u\n"
"failsafe_value: %u\n"),
(unsigned)g.throttle_min,
(unsigned)g.throttle_max,
(unsigned)g.throttle_cruise,
(unsigned)g.fs_throttle_enabled,
(unsigned)g.fs_throttle_value);
print_blanks(2);
}
void Rover::report_compass()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Compass: "));
print_enabled(g.compass_enabled);
// mag declination
cliSerial->printf_P(PSTR("Mag Declination: %4.4f\n"),
(double)degrees(compass.get_declination()));
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);
}
void Rover::report_modes()
{
//print_blanks(2);
cliSerial->printf_P(PSTR("Flight modes\n"));
print_divider();
for(int i = 0; i < 6; i++ ){
print_switch(i, modes[i]);
}
print_blanks(2);
}
/***************************************************************************/
// CLI utilities
/***************************************************************************/
static void
print_PID(PID * pid)
{
cliSerial->printf_P(PSTR("P: %4.3f, I:%4.3f, D:%4.3f, IMAX:%ld\n"),
(double)pid->kP(),
(double)pid->kI(),
(double)pid->kD(),
(long)pid->imax());
}
static void
print_radio_values()
{
cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_steer->radio_min, (int)channel_steer->radio_trim, (int)channel_steer->radio_max);
cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_trim, (int)g.rc_2.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)g.rc_4.radio_min, (int)g.rc_4.radio_trim, (int)g.rc_4.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_switch(uint8_t p, uint8_t m)
{
cliSerial->printf_P(PSTR("Pos %d: "),p);
print_mode(cliSerial, m);
cliSerial->println();
}
static void
print_done()
{
cliSerial->printf_P(PSTR("\nSaved Settings\n\n"));
}
static void
print_blanks(int num)
{
while(num > 0){
num--;
cliSerial->println("");
}
}
static void
print_divider(void)
{
for (int i = 0; i < 40; i++) {
cliSerial->printf_P(PSTR("-"));
}
cliSerial->println("");
}
static int8_t
radio_input_switch(void)
{
int8_t Rover::bouncer = 0;
if (int16_t(channel_steer->radio_in - channel_steer->radio_trim) > 100) {
bouncer = 10;
}
if (int16_t(channel_steer->radio_in - channel_steer->radio_trim) < -100) {
bouncer = -10;
}
if (bouncer >0) {
bouncer --;
}
if (bouncer <0) {
bouncer ++;
}
if (bouncer == 1 || bouncer == -1) {
return bouncer;
} else {
return 0;
}
}
void Rover::zero_eeprom(void)
{
cliSerial->printf_P(PSTR("\nErasing EEPROM\n"));
@ -628,13 +51,4 @@ void Rover::zero_eeprom(void)
cliSerial->printf_P(PSTR("done\n"));
}
void Rover::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

25
APMrover2/system.pde
View File

@ -8,16 +8,10 @@ The init_ardupilot function processes everything we need for an in - air restart
#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
int8_t Rover:: reboot_board(uint8_t argc, const Menu::arg *argv);
// This is the help function
// PSTR is an AVR macro to read strings from flash memory
// printf_P is a version of print_f that reads from flash memory
static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
int8_t Rover::main_menu_help(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf_P(PSTR("Commands:\n"
" logs log readback/setup mode\n"
@ -33,11 +27,11 @@ static int8_t main_menu_help(uint8_t argc, const Menu::arg *argv)
static const struct Menu::command main_menu_commands[] PROGMEM = {
// command function called
// ======= ===============
{"logs", process_logs},
{"setup", setup_mode},
{"test", test_mode},
{"reboot", reboot_board},
{"help", main_menu_help}
{"logs", MENU_FUNC(process_logs)},
{"setup", MENU_FUNC(setup_mode)},
{"test", MENU_FUNC(test_mode)},
{"reboot", MENU_FUNC(reboot_board)},
{"help", MENU_FUNC(main_menu_help)}
};
// Create the top-level menu object.
@ -146,7 +140,7 @@ void Rover::init_ardupilot()
// Register mavlink_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(mavlink_delay_cb, 5);
hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
apm1_adc.Init(); // APM ADC library initialization
@ -187,7 +181,7 @@ void Rover::init_ardupilot()
setup the 'main loop is dead' check. Note that this relies on
the RC library being initialised.
*/
hal.scheduler->register_timer_failsafe(failsafe_check, 1000);
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if CLI_ENABLED == ENABLED
@ -445,8 +439,7 @@ void Rover::check_usb_mux(void)
}
static void
print_mode(AP_HAL::BetterStream *port, uint8_t mode)
void Rover::print_mode(AP_HAL::BetterStream *port, uint8_t mode)
{
switch (mode) {
case MANUAL:

101
APMrover2/test.pde
View File

@ -2,57 +2,35 @@
#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);
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_sonar(uint8_t argc, const Menu::arg *argv);
static int8_t test_mag(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
int8_t Rover:: test_shell(uint8_t argc, const Menu::arg *argv);
#endif
// forward declaration to keep the compiler happy
void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd);
// Creates a constant array of structs representing menu options
// and stores them in Flash memory, not RAM.
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"passthru", test_passthru},
{"failsafe", test_failsafe},
{"relay", test_relay},
{"waypoints", test_wp},
{"modeswitch", test_modeswitch},
{"pwm", MENU_FUNC(test_radio_pwm)},
{"radio", MENU_FUNC(test_radio)},
{"passthru", MENU_FUNC(test_passthru)},
{"failsafe", MENU_FUNC(test_failsafe)},
{"relay", MENU_FUNC(test_relay)},
{"waypoints", MENU_FUNC(test_wp)},
{"modeswitch", MENU_FUNC(test_modeswitch)},
// Tests below here are for hardware sensors only present
// when real sensors are attached or they are emulated
{"gps", test_gps},
{"ins", test_ins},
{"sonartest", test_sonar},
{"compass", test_mag},
{"logging", test_logging},
{"gps", MENU_FUNC(test_gps)},
{"ins", MENU_FUNC(test_ins)},
{"sonartest", MENU_FUNC(test_sonar)},
{"compass", MENU_FUNC(test_mag)},
{"logging", MENU_FUNC(test_logging)},
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
{"shell", test_shell},
{"shell", MENU_FUNC(test_shell)},
#endif
};
// A Macro to create the Menu
MENU(test_menu, "test", test_menu_commands);
static int8_t
test_mode(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_mode(uint8_t argc, const Menu::arg *argv)
{
cliSerial->printf_P(PSTR("Test Mode\n\n"));
test_menu.run();
@ -64,8 +42,7 @@ void Rover::print_hit_enter()
cliSerial->printf_P(PSTR("Hit Enter to exit.\n\n"));
}
static int8_t
test_radio_pwm(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_radio_pwm(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -94,8 +71,7 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
}
static int8_t
test_passthru(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_passthru(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -120,8 +96,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
return 0;
}
static int8_t
test_radio(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_radio(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -157,8 +132,7 @@ test_radio(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_failsafe(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
{
uint8_t fail_test;
print_hit_enter();
@ -212,8 +186,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_relay(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_relay(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -235,8 +208,7 @@ test_relay(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_wp(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_wp(uint8_t argc, const Menu::arg *argv)
{
delay(1000);
@ -253,8 +225,7 @@ test_wp(uint8_t argc, const Menu::arg *argv)
return (0);
}
static void
test_wp_print(const AP_Mission::Mission_Command& cmd)
void Rover::test_wp_print(const AP_Mission::Mission_Command& cmd)
{
cliSerial->printf_P(PSTR("command #: %d id:%d options:%d p1:%d p2:%ld p3:%ld p4:%ld \n"),
(int)cmd.index,
@ -266,8 +237,7 @@ test_wp_print(const AP_Mission::Mission_Command& cmd)
(long)cmd.content.location.lng);
}
static int8_t
test_modeswitch(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_modeswitch(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -292,8 +262,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
/*
test the dataflash is working
*/
static int8_t
test_logging(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_logging(uint8_t argc, const Menu::arg *argv)
{
cliSerial->println_P(PSTR("Testing dataflash logging"));
DataFlash.ShowDeviceInfo(cliSerial);
@ -304,8 +273,7 @@ test_logging(uint8_t argc, const Menu::arg *argv)
//-------------------------------------------------------------------------------------------
// tests in this section are for real sensors or sensors that have been simulated
static int8_t
test_gps(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_gps(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
delay(1000);
@ -333,8 +301,7 @@ test_gps(uint8_t argc, const Menu::arg *argv)
}
}
static int8_t
test_ins(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_ins(uint8_t argc, const Menu::arg *argv)
{
//cliSerial->printf_P(PSTR("Calibrating."));
ahrs.init();
@ -377,9 +344,16 @@ test_ins(uint8_t argc, const Menu::arg *argv)
}
}
void Rover::print_enabled(bool b)
{
if(b)
cliSerial->printf_P(PSTR("en"));
else
cliSerial->printf_P(PSTR("dis"));
cliSerial->printf_P(PSTR("abled\n"));
}
static int8_t
test_mag(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
{
if (!g.compass_enabled) {
cliSerial->printf_P(PSTR("Compass: "));
@ -394,7 +368,6 @@ test_mag(uint8_t argc, const Menu::arg *argv)
ahrs.init();
ahrs.set_fly_forward(true);
ahrs.set_compass(&compass);
report_compass();
// we need the AHRS initialised for this test
ins.init(AP_InertialSensor::COLD_START,
@ -452,8 +425,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
//-------------------------------------------------------------------------------------------
// real sensors that have not been simulated yet go here
static int8_t
test_sonar(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_sonar(uint8_t argc, const Menu::arg *argv)
{
init_sonar();
delay(20);
@ -527,8 +499,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
/*
* run a debug shell
*/
static int8_t
test_shell(uint8_t argc, const Menu::arg *argv)
int8_t Rover::test_shell(uint8_t argc, const Menu::arg *argv)
{
hal.util->run_debug_shell(cliSerial);
return 0;