aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp69
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.hpp11
-rw-r--r--src/modules/fixedwing_backside/fixedwing.cpp23
-rw-r--r--src/modules/fixedwing_backside/fixedwing.hpp14
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp65
-rw-r--r--src/modules/mavlink/missionlib.c10
-rw-r--r--src/modules/px4iofirmware/protocol.h5
-rw-r--r--src/modules/px4iofirmware/px4io.c33
-rw-r--r--src/modules/px4iofirmware/registers.c27
-rw-r--r--src/modules/px4iofirmware/safety.c16
10 files changed, 201 insertions, 72 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 18fb6dcbc..ecca04dd7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -41,6 +41,7 @@
#include "KalmanNav.hpp"
#include <systemlib/err.h>
+#include <geo/geo.h>
// constants
// Titterton pg. 52
@@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
// attitude representations
C_nb(),
q(),
- _accel_sub(-1),
- _gyro_sub(-1),
- _mag_sub(-1),
// subscriptions
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
// publications
_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+ _localPos(&getPublications(), ORB_ID(vehicle_local_position)),
_att(&getPublications(), ORB_ID(vehicle_attitude)),
// timestamps
_pubTimeStamp(hrt_absolute_time()),
@@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
phi(0), theta(0), psi(0),
vN(0), vE(0), vD(0),
lat(0), lon(0), alt(0),
+ lat0(0), lon0(0), alt0(0),
// parameters for ground station
_vGyro(this, "V_GYRO"),
_vAccel(this, "V_ACCEL"),
@@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
lon = 0.0f;
alt = 0.0f;
- // gyro, accel and mag subscriptions
- _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
- _accel_sub = orb_subscribe(ORB_ID(sensor_accel));
- _mag_sub = orb_subscribe(ORB_ID(sensor_mag));
-
- struct accel_report accel;
- orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
-
- struct mag_report mag;
- orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
-
// initialize rotation quaternion with a single raw sensor measurement
- q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
+ _sensors.update();
+ q = init(
+ _sensors.accelerometer_m_s2[0],
+ _sensors.accelerometer_m_s2[1],
+ _sensors.accelerometer_m_s2[2],
+ _sensors.magnetometer_ga[0],
+ _sensors.magnetometer_ga[1],
+ _sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
@@ -252,11 +248,21 @@ void KalmanNav::update()
setLatDegE7(_gps.lat);
setLonDegE7(_gps.lon);
setAltE3(_gps.alt);
+ // set reference position for
+ // local position
+ lat0 = lat;
+ lon0 = lon;
+ alt0 = alt;
+ // XXX map_projection has internal global
+ // states that multiple things could change,
+ // should make map_projection take reference
+ // lat/lon and not have init
+ map_projection_init(lat0, lon0);
_positionInitialized = true;
warnx("initialized EKF state with GPS\n");
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
double(vN), double(vE), double(vD),
- lat, lon, alt);
+ lat, lon, double(alt));
}
// prediction step
@@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
{
using namespace math;
- // position publication
+ // global position publication
_pos.timestamp = _pubTimeStamp;
_pos.time_gps_usec = _gps.timestamp_position;
_pos.valid = true;
@@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
_pos.vz = vD;
_pos.yaw = psi;
+ // local position publication
+ float x;
+ float y;
+ bool landed = alt < (alt0 + 0.1); // XXX improve?
+ map_projection_project(lat, lon, &x, &y);
+ _localPos.timestamp = _pubTimeStamp;
+ _localPos.xy_valid = true;
+ _localPos.z_valid = true;
+ _localPos.v_xy_valid = true;
+ _localPos.v_z_valid = true;
+ _localPos.x = x;
+ _localPos.y = y;
+ _localPos.z = alt0 - alt;
+ _localPos.vx = vN;
+ _localPos.vy = vE;
+ _localPos.vz = vD;
+ _localPos.yaw = psi;
+ _localPos.xy_global = true;
+ _localPos.z_global = true;
+ _localPos.ref_timestamp = _pubTimeStamp;
+ _localPos.ref_lat = getLatDegE7();
+ _localPos.ref_lon = getLonDegE7();
+ _localPos.ref_alt = 0;
+ _localPos.landed = landed;
+
// attitude publication
_att.timestamp = _pubTimeStamp;
_att.roll = phi;
@@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
// selectively update publications,
// do NOT call superblock do-all method
- if (_positionInitialized)
+ if (_positionInitialized) {
_pos.update();
+ _localPos.update();
+ }
if (_attitudeInitialized)
_att.update();
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 799fc2fb9..a69bde1a6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -52,6 +52,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/parameter_update.h>
@@ -142,12 +143,8 @@ protected:
control::UOrbSubscription<parameter_update_s> _param_update; /**< parameter update sub. */
// publications
control::UOrbPublication<vehicle_global_position_s> _pos; /**< position pub. */
+ control::UOrbPublication<vehicle_local_position_s> _localPos; /**< local position pub. */
control::UOrbPublication<vehicle_attitude_s> _att; /**< attitude pub. */
-
- int _accel_sub; /**< Accelerometer subscription */
- int _gyro_sub; /**< Gyroscope subscription */
- int _mag_sub; /**< Magnetometer subscription */
-
// time stamps
uint64_t _pubTimeStamp; /**< output data publication time stamp */
uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -164,8 +161,10 @@ protected:
float phi, theta, psi; /**< 3-2-1 euler angles */
float vN, vE, vD; /**< navigation velocity, m/s */
double lat, lon; /**< lat, lon radians */
- float alt; /**< altitude, meters */
// parameters
+ float alt; /**< altitude, meters */
+ double lat0, lon0; /**< reference latitude and longitude */
+ float alt0; /**< refeerence altitude (ground height) */
control::BlockParamFloat _vGyro; /**< gyro process noise */
control::BlockParamFloat _vAccel; /**< accelerometer process noise */
control::BlockParamFloat _rMag; /**< magnetometer measurement noise */
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index d65045d68..6dc19df41 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
- if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here?
+ if (_status.main_state == MAIN_STATE_AUTO) {
+ // TODO use vehicle_control_mode here?
// update guidance
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
}
@@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
// the setpoint should update to loitering around this position
// handle autopilot modes
- if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
- _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
+ if (_status.main_state == MAIN_STATE_AUTO) {
- // update guidance
- _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
-
- // calculate velocity, XXX should be airspeed, but using ground speed for now
- // for the purpose of control we will limit the velocity feedback between
+ // calculate velocity, XXX should be airspeed,
+ // but using ground speed for now for the purpose
+ // of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
@@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update()
// a first binary release can be targeted.
// This is not a hack, but a design choice.
- /* do not limit in HIL */
+ // do not limit in HIL
if (_status.hil_state != HIL_STATE_ON) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
_actuators.control[CH_THR] : _manual.throttle;
}
- } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here?
+ } else if (_status.main_state == MAIN_STATE_MANUAL) {
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
- } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here?
+
+ } else if (_status.main_state == MAIN_STATE_SEATBELT ||
+ _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 3876e4630..567efeb35 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -255,13 +255,13 @@ private:
BlockWaypointGuidance _guide;
// block params
- BlockParam<float> _trimAil;
- BlockParam<float> _trimElv;
- BlockParam<float> _trimRdr;
- BlockParam<float> _trimThr;
- BlockParam<float> _trimV;
- BlockParam<float> _vCmd;
- BlockParam<float> _crMax;
+ BlockParamFloat _trimAil;
+ BlockParamFloat _trimElv;
+ BlockParamFloat _trimRdr;
+ BlockParamFloat _trimThr;
+ BlockParamFloat _trimV;
+ BlockParamFloat _vCmd;
+ BlockParamFloat _crMax;
struct pollfd _attPoll;
vehicle_global_position_set_triplet_s _lastPosCmd;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c51a6de08..bfccd5fb0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -72,6 +72,7 @@
#include <systemlib/airspeed.h>
#include <mavlink/mavlink_log.h>
#include <commander/px4_custom_mode.h>
+#include <geo/geo.h>
__BEGIN_DECLS
@@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
static struct offboard_control_setpoint_s offboard_control_sp;
struct vehicle_global_position_s hil_global_pos;
+struct vehicle_local_position_s hil_local_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t pub_hil_local_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
static orb_advert_t pub_hil_sensors = -1;
@@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
static orb_advert_t vicon_position_pub = -1;
static orb_advert_t telemetry_status_pub = -1;
+// variables for HIL reference position
+static int32_t lat0 = 0;
+static int32_t lon0 = 0;
+static double alt0 = 0;
+
static void
handle_message(mavlink_message_t *msg)
{
@@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
- hil_global_pos.valid = true;
- hil_global_pos.lat = hil_state.lat;
- hil_global_pos.lon = hil_state.lon;
- hil_global_pos.alt = hil_state.alt / 1000.0f;
- hil_global_pos.vx = hil_state.vx / 100.0f;
- hil_global_pos.vy = hil_state.vy / 100.0f;
- hil_global_pos.vz = hil_state.vz / 100.0f;
-
- /* set timestamp and notify processes (broadcast) */
- hil_global_pos.timestamp = hrt_absolute_time();
+ uint64_t timestamp = hrt_absolute_time();
+ // publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+ // global position packet
+ hil_global_pos.timestamp = timestamp;
+ hil_global_pos.valid = true;
+ hil_global_pos.lat = hil_state.lat;
+ hil_global_pos.lon = hil_state.lon;
+ hil_global_pos.alt = hil_state.alt / 1000.0f;
+ hil_global_pos.vx = hil_state.vx / 100.0f;
+ hil_global_pos.vy = hil_state.vy / 100.0f;
+ hil_global_pos.vz = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
+ // publish local position
+ if (pub_hil_local_pos > 0) {
+ float x;
+ float y;
+ bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
+ double lat = hil_state.lat*1e-7;
+ double lon = hil_state.lon*1e-7;
+ map_projection_project(lat, lon, &x, &y);
+ hil_local_pos.timestamp = timestamp;
+ hil_local_pos.xy_valid = true;
+ hil_local_pos.z_valid = true;
+ hil_local_pos.v_xy_valid = true;
+ hil_local_pos.v_z_valid = true;
+ hil_local_pos.x = x;
+ hil_local_pos.y = y;
+ hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
+ hil_local_pos.vx = hil_state.vx/100.0f;
+ hil_local_pos.vy = hil_state.vy/100.0f;
+ hil_local_pos.vz = hil_state.vz/100.0f;
+ hil_local_pos.yaw = hil_attitude.yaw;
+ hil_local_pos.xy_global = true;
+ hil_local_pos.z_global = true;
+ hil_local_pos.ref_timestamp = timestamp;
+ hil_local_pos.ref_lat = hil_state.lat;
+ hil_local_pos.ref_lon = hil_state.lon;
+ hil_local_pos.ref_alt = alt0;
+ hil_local_pos.landed = landed;
+ orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
+ } else {
+ pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+ lat0 = hil_state.lat;
+ lon0 = hil_state.lon;
+ alt0 = hil_state.alt / 1000.0f;
+ map_projection_init(hil_state.lat, hil_state.lon);
+ }
+
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index fa23f996f..124b3b2ae 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -276,7 +276,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
next_setpoint_index = index + 1;
}
- while (next_setpoint_index < wpm->size - 1) {
+ while (next_setpoint_index < wpm->size) {
if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
@@ -301,7 +301,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[last_setpoint_index].z;
sp.altitude_is_relative = false;
- sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(wpm->waypoints[last_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[last_setpoint_index].param1,
wpm->waypoints[last_setpoint_index].param2,
wpm->waypoints[last_setpoint_index].param3,
@@ -317,7 +317,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
sp.altitude = wpm->waypoints[next_setpoint_index].z;
sp.altitude_is_relative = false;
- sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(wpm->waypoints[next_setpoint_index].param4 / 180.0f * M_PI_F);
set_special_fields(wpm->waypoints[next_setpoint_index].param1,
wpm->waypoints[next_setpoint_index].param2,
wpm->waypoints[next_setpoint_index].param3,
@@ -343,7 +343,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = true;
- sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize publication if necessary */
@@ -364,7 +364,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.x = param5_lat_x;
sp.y = param6_lon_y;
sp.z = param7_alt_z;
- sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+ sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
/* Initialize publication if necessary */
if (local_position_setpoint_pub < 0) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 5e5396782..cffabbb45 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -186,6 +186,11 @@ enum { /* DSM bind states */
/* 8 */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
+#define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */
+#define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */
+
+#define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */
+
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index ff9eecd74..745bd5705 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -45,6 +45,7 @@
#include <string.h>
#include <poll.h>
#include <signal.h>
+#include <crc32.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
@@ -117,6 +118,29 @@ show_debug_messages(void)
}
}
+static void
+heartbeat_blink(void)
+{
+ static bool heartbeat = false;
+ LED_BLUE(heartbeat = !heartbeat);
+}
+
+
+static void
+calculate_fw_crc(void)
+{
+#define APP_SIZE_MAX 0xf000
+#define APP_LOAD_ADDRESS 0x08001000
+ // compute CRC of the current firmware
+ uint32_t sum = 0;
+ for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
+ uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
+ sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
+ }
+ r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
+ r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
+}
+
int
user_start(int argc, char *argv[])
{
@@ -129,6 +153,9 @@ user_start(int argc, char *argv[])
/* configure the high-resolution time/callout interface */
hrt_init();
+ /* calculate our fw CRC so FMU can decide if we need to update */
+ calculate_fw_crc();
+
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
@@ -201,6 +228,7 @@ user_start(int argc, char *argv[])
*/
uint64_t last_debug_time = 0;
+ uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
@@ -216,6 +244,11 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
+ if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) {
+ last_heartbeat_time = hrt_absolute_time();
+ heartbeat_blink();
+ }
+
#if 0
/* check for debug activity */
show_debug_messages();
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 86a40bc22..1a8519aec 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -45,6 +45,8 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
+#include <systemlib/systemlib.h>
+#include <stm32_pwr.h>
#include "px4io.h"
#include "protocol.h"
@@ -154,6 +156,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
#endif
[PX4IO_P_SETUP_SET_DEBUG] = 0,
+ [PX4IO_P_SETUP_REBOOT_BL] = 0,
+ [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@@ -501,6 +505,29 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
+ case PX4IO_P_SETUP_REBOOT_BL:
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ // don't allow reboot while armed
+ break;
+ }
+
+ // check the magic value
+ if (value != PX4IO_REBOOT_BL_MAGIC)
+ break;
+
+ // note that we don't set BL_WAIT_MAGIC in
+ // BKP_DR1 as that is not necessary given the
+ // timing of the forceupdate command. The
+ // bootloader on px4io waits for enough time
+ // anyway, and this method works with older
+ // bootloader versions (tested with both
+ // revision 3 and revision 4).
+
+ up_systemreset();
+ break;
+
case PX4IO_P_SETUP_DSM:
dsm_bind(value & 0x0f, (value >> 4) & 7);
break;
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 95335f038..cdb54a80a 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -77,7 +77,6 @@ static unsigned blink_counter = 0;
static bool safety_button_pressed;
static void safety_check_button(void *arg);
-static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
@@ -86,9 +85,6 @@ safety_init(void)
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
- /* arrange for the heartbeat handler to be called at 4Hz */
- hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
-
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
@@ -164,16 +160,6 @@ safety_check_button(void *arg)
}
static void
-heartbeat_blink(void *arg)
-{
- static bool heartbeat = false;
-
- /* XXX add flags here that need to be frobbed by various loops */
-
- LED_BLUE(heartbeat = !heartbeat);
-}
-
-static void
failsafe_blink(void *arg)
{
/* indicate that a serious initialisation error occured */
@@ -192,4 +178,4 @@ failsafe_blink(void *arg)
}
LED_AMBER(failsafe);
-} \ No newline at end of file
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-25 10:11:20 +0000