diff --git a/skyward-boardcore b/skyward-boardcore index 9973d441dcbf2e88a7ba52b2d35a923da9f5b55a..56f0491004203cfee6d417281a82aedb612ee8d7 160000 --- a/skyward-boardcore +++ b/skyward-boardcore @@ -1 +1 @@ -Subproject commit 9973d441dcbf2e88a7ba52b2d35a923da9f5b55a +Subproject commit 56f0491004203cfee6d417281a82aedb612ee8d7 diff --git a/src/boards/Payload/Radio/Radio.cpp b/src/boards/Payload/Radio/Radio.cpp index 5f5fe24c48639a225311df3788a056e70b412136..cba577c5dfaca63f65df524e61ed0f461a14eab2 100644 --- a/src/boards/Payload/Radio/Radio.cpp +++ b/src/boards/Payload/Radio/Radio.cpp @@ -394,6 +394,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver, mavlink_msg_set_reference_altitude_tc_get_ref_altitude(&msg); NASController::getInstance().setReferenceAltitude(altitude); + Sensors::getInstance().pitotSetReferenceAltitude(altitude); break; } case MAVLINK_MSG_ID_SET_REFERENCE_TEMPERATURE_TC: @@ -402,6 +403,7 @@ void Radio::handleMavlinkMessage(MavDriver* driver, mavlink_msg_set_reference_temperature_tc_get_ref_temp(&msg); NASController::getInstance().setReferenceTemperature(temperature); + Sensors::getInstance().pitotSetReferenceTemperature(temperature); break; } case MAVLINK_MSG_ID_SET_DEPLOYMENT_ALTITUDE_TC: diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp index ec4c9a685eb93e3a2831f54d2279bfcc4850f555..960a28b290b22dd8d69325910bc19ebc72c7b00a 100644 --- a/src/boards/Payload/Sensors/Sensors.cpp +++ b/src/boards/Payload/Sensors/Sensors.cpp @@ -169,6 +169,28 @@ void Sensors::calibrate() calibrating = true; } +void Sensors::pitotSetReferenceAltitude(float altitude) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = pitot->getReferenceValues(); + reference.refAltitude = altitude; + pitot->setReferenceValues(reference); +} + +void Sensors::pitotSetReferenceTemperature(float temperature) +{ + // Need to pause the kernel because the only invocation comes from the radio + // which is a separate thread + miosix::PauseKernelLock l; + + ReferenceValues reference = pitot->getReferenceValues(); + reference.refTemperature = temperature + 273.15f; + pitot->setReferenceValues(reference); +} + std::map<string, bool> Sensors::getSensorsState() { std::map<string, bool> sensorsState; diff --git a/src/boards/Payload/Sensors/Sensors.h b/src/boards/Payload/Sensors/Sensors.h index 0a478e1922d04d3662b6046386b2edbb0d782723..97b8397005400411aeef050eed5f340ae22b4f91 100644 --- a/src/boards/Payload/Sensors/Sensors.h +++ b/src/boards/Payload/Sensors/Sensors.h @@ -73,6 +73,9 @@ public: */ void calibrate(); + void pitotSetReferenceAltitude(float altitude); + void pitotSetReferenceTemperature(float temperature); + std::map<string, bool> getSensorsState(); private: