From b731ebfd9efb19565f9a3002e9238e5020cf284a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 12 May 2015 17:00:25 +1000 Subject: [PATCH] Rover: coversion to class now complete --- APMrover2/APMrover2.pde | 87 +++-- APMrover2/GCS_Mavlink.pde | 245 ++++++++------- APMrover2/Log.pde | 48 ++- APMrover2/Parameters.pde | 10 +- APMrover2/Rover.cpp | 90 ------ APMrover2/Rover.h | 155 +++++++-- APMrover2/Rover.pde | 61 ++++ APMrover2/commands.pde | 2 +- APMrover2/commands_logic.pde | 15 +- APMrover2/compat.h | 3 - APMrover2/failsafe.pde | 9 +- APMrover2/sensors.pde | 2 +- APMrover2/setup.pde | 592 +---------------------------------- APMrover2/system.pde | 25 +- APMrover2/test.pde | 101 +++--- 15 files changed, 458 insertions(+), 987 deletions(-) delete mode 100644 APMrover2/Rover.cpp create mode 100644 APMrover2/Rover.pde diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index d45516f1a4..c0d5caf214 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -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 -#include -#include - -// Libraries #include #include #include @@ -126,11 +101,57 @@ #include // 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; icrosstrack_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; iprintf_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 diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 860ee2a3fb..8472113db4 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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), diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp deleted file mode 100644 index c34a0aca4d..0000000000 --- a/APMrover2/Rover.cpp +++ /dev/null @@ -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 . -*/ -/* - 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), -{ -} diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 92774aecc6..2002eac84e 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -17,9 +17,85 @@ main Rover class, containing all vehicle specific state */ +#include +#include + +// Libraries +#include +#include +#include +#include +#include +#include +#include // ArduPilot GPS library +#include // ArduPilot Mega Analog to Digital Converter Library +#include +#include +#include // ArduPilot Mega Magnetometer Library +#include // ArduPilot Mega Vector/Matrix math Library +#include // Inertial Sensor (uncalibated IMU) Library +#include // ArduPilot Mega DCM Library +#include +#include // Mission command library +#include +#include +#include // PID library +#include // RC Channel Library +#include // Range finder library +#include // Filter library +#include // Filter library - butterworth filter +#include // FIFO buffer library +#include // Mode Filter from Filter library +#include // Mode Filter from Filter library +#include // APM relay +#include +#include // Camera/Antenna mount +#include // Camera triggering +#include // MAVLink GCS definitions +#include // Serial manager library +#include // needed for AHRS build +#include // needed for AHRS build +#include +#include // RC input mapping library +#include +#include // main loop scheduler +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "compat.h" + +#include // Notify library +#include // Battery monitor library +#include // Optical Flow library + +// Configuration +#include "config.h" + +// Local modules +#include "defines.h" +#include "Parameters.h" +#include "GCS.h" + +#include // 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) diff --git a/APMrover2/Rover.pde b/APMrover2/Rover.pde new file mode 100644 index 0000000000..0ad1ff7aa0 --- /dev/null +++ b/APMrover2/Rover.pde @@ -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 . +*/ +/* + 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) +{ +} diff --git a/APMrover2/commands.pde b/APMrover2/commands.pde index 8bf0346110..b9c91ebd0b 100644 --- a/APMrover2/commands.pde +++ b/APMrover2/commands.pde @@ -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 diff --git a/APMrover2/commands_logic.pde b/APMrover2/commands_logic.pde index 19bc696473..f72a1a4d25 100644 --- a/APMrover2/commands_logic.pde +++ b/APMrover2/commands_logic.pde @@ -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)) { diff --git a/APMrover2/compat.h b/APMrover2/compat.h index 64b7a7e6e2..37ba4b7d8f 100644 --- a/APMrover2/compat.h +++ b/APMrover2/compat.h @@ -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__ diff --git a/APMrover2/failsafe.pde b/APMrover2/failsafe.pde index c83cd54bb4..0b879c02c2 100644 --- a/APMrover2/failsafe.pde +++ b/APMrover2/failsafe.pde @@ -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(); +} diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 7ff02114d4..03332e14c3 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -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; diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 602cdb1257..52497270d8 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -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 \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 diff --git a/APMrover2/system.pde b/APMrover2/system.pde index ffaa21946c..98b3849997 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -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: diff --git a/APMrover2/test.pde b/APMrover2/test.pde index f47c3b536f..ce49733649 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -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;